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Abstract 

A  current  project  of  great  import  to  the  National 
Aeronautics  and  Space  Administration  is  the  development  of 
an  automated  docking  and  tracking  system  to  facilitate 
autonomous  operations  in  space.  Such  a  system  would  have  to 
be  able  to  determine  the  relative  positions,  velocities,  and 
orientations  of  a  multitude  of  bodies. 

A  stereoscopic  vision  system  was  developed  to 
implement  an  automated  docking  solution.  This  system  used 
artificial  neural  networks  to  identify  beacons  or  fixed 
points  on  the  objects  to  be  tracked.  Specifically,  this 
research  sought  to  solve  the  problems  inherent  in  the 
planned  1995  docking  of  the  U.S.  Space  Shuttle  to  the  former 
Soviet  Space  Station  Mir. 

In  addition  to  the  basics  of  an  optical  ranging  and 
object-recognition  system,  a  simple  user  interface  for 
operation  monitoring  was  designed.  Specifically,  the 
coordinates  of  the  space  station,  shuttle  waypoints,  and 
smooth  trajectory  position,  velocity,  and  acceleration 
information  were  calculated  and  displayed.  A  mock  docking 
was  also  simulated,  with  the  vision  system  providing  the 
range  and  orientation  data.  By  placing  the  vision  system  at 
known  coordinates  and  checking  its  computed  trajectory,  the 
accuracy  of  the  algorithms  and  given  hardware  were  checked. 
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Note:  The  majority  of  the  ideas  and  algorithms  presented  in 
this  paper  were  developed  either  in  collaboration  with  or 
solely  by  Assistant  Professor  William  I.  Clement. 
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Motivation  and  Purpose 

Currently,  the  policy  of  maintaining  humans  in  space  is 
called  into  question  by  the  cost  of  transporting  the 
essentials  for  human  survival  into  space.  Other  than  for 
reasons  of  medical  research  and  publicity,  humans  in  space 
are  neither  vital  nor  economical  at  this  time.  The  use  of 
robotics  to  accomplish  some  human  missions  could  drastically 
cut  the  costs  of  future  operations  in  space  exploration  and 
development  of  space  for  humanity.  One  prerequisite  to 
practical  robot  operations  in  space  is  a  flexible  sensor 
system  with  which  more  complete  automation  can  be 
accomplished.  Systems  using  the  spectrum  visible  to  the 
human  eye  are  the  sensors  most  conducive  to  operational 
systems  designed  to  mimic  human  functions. 

The  use  of  robotics  is  definitely  not  limited  to  space. 
Aerial  or  even  undersea  robotic  systems  would  require  the 
same  type  of  visual  sensory  data  to  be  truly  effective. 
Furthermore,  automation  enhanced  by  machine  vision  has 
already  revolutionized  some  aspects  of  our  lives.  The 
automated  car  assembly  line  operated  by  robots  is  one 
example . 

However,  before  truly  effective  multi-purpose  robots 
can  be  made,  they  must  be  given  the  essential  tools  through 
which  they  can  have  awareness  of  their  surrounding 
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environment.  Of  the  many  sensor  technologies  available, 
vision  will  be  the  most  important  because  of  its 
flexibility.  Robots  must  oe  able  to  'see'  the  objects 
around  them,  determine  if  and  how  they  are  moving,  and 
identify  them  as  well.  Once  a  robot  can  open  its  'eyes', 
analyze  its  surroundings,  and  identify  the  objects  in  its 
environment,  it  will  be  capable  of  performing  innumerable 
tasks. 

The  ultimate  goal  of  this  Trident  project  was  the 
design  of  a  vision  system  capable  of  locating,  tracking,  and 
recognizing  objects  in  its  environment.  These  goals  may  be 
pursued  to  widely  varying  degrees  of  refinement  and  success. 
The  technology  explored  by  this  project  will  have  vital  uses 
in  future  exploration  and  development  in  all  realms  - 
atmospheric,  terrestrial,  space,  and  aquatic.  The  automated 
docking  and  tracking  system  described  here  is  a  substantial 
first  step  to  such  a  system. 

Approach 

In  the  docking  situation  simulated,  a  space  shuttle 
must  rendezvous  and  dock  with  a  space  station,  using  a 
stereoscopic  vision  system  connected  to  its  flight  computer 
as  a  sensor.  The  space  shuttle  has  already  been  maneuvered 
to  the  general  vicinity  of  the  station,  and  to  the  general 
area  of  the  side  of  the  station  with  which  it  is  to  dock. 
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Figure  1  shows  a  nearly  completed  docking  between  the  space 
shuttle  and  a  futuristic  space  station. 

The  system  was  designed  to  isolate  the  basic  problems 
encountered  in  a  stereoscopic  vision  system.  This  project 
was  a  simplification  of  the  general  case  by  limiting  the 
area  in  which  the  system  had  to  search  for  its  target,  and 
limiting  the  target  to  a  specific  beacon  of  known  and  unique 
characteristics.  These  restrictions  were  imposed  in  cart  by 
the  hardware  used,  as  seen  in  the  field  of  \iew  of  the 
camera,  and  in  part  by  lack  of  sufficient  progress  made  thus 
far  in  some  vision  technologies,  such  as  edge  detection. 
Later  work  was  directed  towards  the  optimization  of  this 
system,  and  development  of  algorithms  to  fine-tune  the 
tracking  process. 

The  majority  of  the  work  on  this  project  was  done  in 
computer  code.  The  final  product  was  not  hardware,  but  a 
set  of  algorithms  in  the  form  of  a  program,  which  was 
adaptable  to  more  advanced  hardware  than  that  of  the  current 
system  setup.  All  algorithms,  including  the  interaction 
with  the  frame-grabbing  and  frame-processing  boards  and  the 
software-based  artificial  neural  network,  were  written  in 
the  ' C'  programming  language . 

The  automatic  range-finding  algorithms  were  relatively 
straightforward.  The  goal  of  the  investigator  was  to 
automate  this  process  to  the  greatest  extent.  Further 
development  of  the  project  was  initially  planned  to 
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incorporate  the  development  and  use  of  specific  geometries 
of  visual  beacons  which  could  enhance  the  ranging  process 
and  help  automate  the  finding  of  corresponding  points  on 
each  image.  This  approach  was  later  deemed  unnecessary,  as 
there  already  exist  on  space  stations  many  distinctive 
objects  which  could  be  isolated  by  vision  systems. 

Instead,  the  use  of  neural  networks  and  pattern 
recognition  was  incorporated  to  identify  the  beacons,  and 
»ven  the  objects  themselves  with  speed  and  accuracy.  The 
goal  of  this  project  was  the  production  of  a  system  which, 
when  given  a  target,  would  automatically  lock  onto  it  and 
determine  its  range,  bearing,  orientation,  and  identity,  and 
compute  waypoints  and  a  trajectory  to  it. 

Why  the  Optical  Approach?  Advantages  and  Problems 

The  optical  approach  to  this  problem  was  superior  to 
the  radar  approach.  First,  at  close  proximity,  radar 
interference  with  objects  and  with  other  similar  systems 
caused  significant  problems.  In  this  case,  the  passive 
nature  of  the  optical  solution  was  also  superior.  An 
optical  system  was  far  more  effective  at  tracking  a 
multitude  of  objects  at  one  time.  It  was  limited  mainly  by 
its  field  of  view  and  computer  processing  speed.  Second, 
the  detail  accommodated  by  optical  systems  allowed  much  more 
precise  mapping  of  objects  in  order  to  determine  object 


orientation.  The  detail  provided  by  visual  data  could  also 
be  exploited  to  develop  a  pattern  recognition  approach  to 
object  identification  and/or  categorization. 

While  the  development  of  a  docking  system  was  the 
primary  goal  of  this  project,  the  system  has  a  variety  of 
other  capabilities.  One  is  the  capability  to  track  a 
multitude  of  smaller  objects,  such  as  the  tools  and 
components  of  a  space  structure  under  construction.  Further 
refinement  of  this  vision  system  to  enhance  tracking  is 
facilitated  by  the  current  approach.  By  using  a  pattern 
recognition  system  based  on  a  simple  computer-based  neural 
network,  objects  on  the  space  station  (for  orientation 
purposes)  and  independent  objects  in  proximity  of  the  system 
can  be  identified.  This  combination  of  capabilities  allows 
an  automated  system  to  have  awareness  of  its  environment;  a 
prerequisite  to  fully  automated  operations  in  space. 

There  were  also  several  problems  with  vision  systems 
which  were  either  briefly  considered  in  this  project  or 
relegated  to  ongoing  vision  research.  These  issues  included 
interference  (in  the  form  of  glare),  image  improvement,  and 
edge  detection.  The  first  two  obstacles  were  found  to  be 
partially  solved  by  the  use  of  the  artificial  neural  network 
approach.  The  interpolating  nature  of  solutions  achieved  by 
artificial  neural  networks  compensates  for  some  degree  of 
error  and  noise  interference  in  object  descriptors . 

However,  accurate  edge  detection,  a  subject  of  extensive 


9 


research,  will  be  vital  to  the  success  of  this  project . 

Obviously,  use  of  this  system  is  precluded  in  regions 
where  sunlight  was  either  unavailable  or  eclipsed  by  the 
space  station  itself.  Spotlights  could  partially  solve  this 
problem.  Examination  of  current  space  operations  shows 
similar  limitations.  The  astronauts  must  have  light  to  see 
and  the  sun  must  not  be  in  a  position  which  blinds  them. 
Therefore,  this  system  could  at  least  be  used  in  the 
majority  of  docking  situations  already  encountered. 


The  Mir  Space  Station 

The  Mir  Space  Station  was  launched  on  February  20, 

1986.  The  name,  meaning  "Peace",  signified  the  Soviets' 
opposition  to  the  Strategic  Defense  Initiative.  [1]  Mir' s 
commonality  with  its  predecessors,  the  Salyut  stations,  was 
determined  primarily  because  of  the  dimensions  of  its  launch 
vehicle,  which  were  the  same.  However,  Mir  featured  a 
significant  improvement:  six  docking  ports,  rather  than  the 
two  of  the  Salyut.  While  Salyuts  had  docking  ports  at  each 
end  of  their  cylindrical  structures,  Mir  had  four  more  ports 
spaced  around  the  node  of  its  forward  transfer  compartment. 
[2]  Its  20  metric  ton  initial  form  was  designed  to  be 
expanded  by  means  of  these  ports  to  a  120  ton,  six-module 
configuration,  some  of  which  is  shown  in  Figure  2. 

To  date,  three  additional  modules  have  been  added.  The 
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Kvant-1  ("Quantum")  astrophysics  module  was  docked  to  the 
stern  port,  with  some  difficulty,  on  April  12,  1987.  This 
11-ton  module  incorporated  advanced  equipment  from  the 
Netherlands,  Great  Britain,  the  European  Space  Agency,  and 
West  Germany,  in  addition  to  Soviet  equipment.  Kvant-2  (20 
tons)  was  manually  docked  to  one  of  the  four  forward  radial 
ports  in  December,  1989.  Twenty-ton  Kristall,  known  as  the 
T-module,  was  added  to  the  opposite  radial  port  in  1990. 
Additionally,  Kristall,  a  materials  processing  and 
biotechnology  module,  was  to  function  as  a  docking  port  for 
the  Soviet  Buran  space  shuttle  and  other  future 
international  manned  spacecraft.  This  module  is  equipped 
with  the  Androgynous-Peripheral  Docking  System  (APDS-89) , 
which  was  based  on  the  system  used  in  the  U.S.  -  U.S.S.R. 
Apollo-Soyuz  Test  Project  of  1975.  One  of  the  U.S.  Space 
Shuttles  will  dock  with  this  module  in  1995.  [3] 

In  addition  to  the  four  current  modules,  Mir  is 
occasionally  visited  by  Prognoz  M  ("Progress")  and  Soyuz  TM 
("Union")  vehicles,  which  respectively  provide  the  Salyut 
and  Mir  space  stations  with  resupply  and  crew  transfer 
capabilities.  Each  weighs  seven  tons.  When  the  U.S. 
docking  occurs  with  Mir,  the  entire  station  complex  will 
weigh  between  71  and  85  metric  tons.  [1,2,3]  This  mass, 
coupled  with  the  U.S.  Space  Shuttle's  variable  mass, 
(between  73  and  102  metric  tons)  poses  a  difficult  docking 
problem  to  solve  because  of  the  large  momenta.  Both 
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vehicles  are  moveable  under  their  own  power.  Only  one, 
however,  the  shuttle,  will  maneuver  independently.  Previous 
U.S.  and  Russian  docking  operations  have  relied  on  manual 
operation  of  the  vehicles,  involving  visual  cues  or  radio 
signals.  Examples  include  the  Igla  and  newer  Kurs 
rendezvous  and  docking  systems.  [3] 


The  D.S.  Space  Shuttle 

The  U.S.  Space  Shuttles  were  designed  with  the 
capability  to  dock  with  other  objects  in  space.  Within 
their  4.6  by  18.3  meter  cargo  bays  [4],  the  shuttles  can 
carry  L-shaped  docking  ports  which  lie  flat  at  launch  and 
rotate  to  a  vertical  protruding  position  upon  commencement 
of  docking  operations.  [5]  Access  to  and  from  the  space 
shuttle  is  available  at  the  aft  of  the  mid  deck,  through  a 
2.1  meter  high  airlock,  which  has  an  inside  diameter  of  1.6 
meters.  The  aft  flight  deck  station  of  the  shuttle  serves  as 
the  command  center  for  rendezvous  maneuvering,  and  has  hand 
controls  and  attitude  indicators  for  that  purpose.  In 
addition,  the  shuttle  has  a  pair  of  windows  facing  out  of 
its  top,  and  a  pair  of  windows  facing  aft  into  its  cargo  bay 
from  which  crew  members  can  supervise  and  control  docking 
and  robot  manipulator  operations.  [4] 

To  provide  thrust  and  attitude  control  in  orbit,  space 


shuttles  are  equipped  with  two  Orbital  Maneuvering  System 
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(OMS)  Engines  to  the  left  and  right  of  the  upper  main 
engine,  38  primary  Reaction  Control  System  (RCS)  jets  (14  in 
the  nose,  and  12  each  in  the  OMS  pods) ,  and  6  vernier  RCS 
engines  (2  in  the  nose,  2  each  in  the  OMS  pods) .  The  OMS 
engines  provide  a  thrust  of  6,000  pounds  for  major 
trajectory  changes  (they  can  be  swivelled  8  degrees  in  any 
direction),  the  primaries  870  pounds,  and  the  verniers  25 
pounds  for  fine  control.  The  shuttle  computer  can  fire 
these  engines  to  make  large  orbital  changes  for  rendezvous, 
or  maintain  precision  pointing  with  an  accuracy  of  less  than 
half  a  degree.  [4,5] 


The  Shuttle  Camera  System 

At  all  four  corners  of  the  15  by  60  feet  cargo  bay  are 
surveillance  cameras  with  a  nominal  purpose  of  monitoring 
the  robot  manipulator's  operations  with  the  payload.  These 
four  cameras  are  monitored  from  the  aft  flight  deck  at  the 
payload  operator's  station.  The  two  cameras  placed  nearest 
this  station  are  shown  in  Figure  3.  In  addition,  selection 
processes  are  currently  underway  to  choose  surveillance 
cameras  for  use  on  board  Space  Station  Freedom  for  both 
monitoring  and  docking  purposes.  Cameras  similar  to  these 
will  likely  be  mounted  on  either  side  of  the  docking 
assembly  within  the  shuttle  bay  as  well  to  further  aid 
rendezvous  operations.  This  assortment  of  cameras. 
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totalling  six  will  include  the  existing  four  in  the  shuttle 
bav.  They  provide  an  excellent  sensor  array  for  a 
,ereoscopic  docking  system,  with  minimal  adjustment  of  the 
available  hardware.  To  maximize  existing  hardware  to 
accomplish  stereo  vision  and  object  recognition,  this 
project  simulated  the  use  of  camera  pairs  already  on  board 
the  shuttle. 


Experimental  System  Components 

The  system  used  in  this  investigation  consisted  of : 

(1)  Two  Panasonic  Mini-Cameras  with  variable  apertures 
and  assorted  lenses.  The  lenses  used  have  focal  lengths  of 
7.3  mm . 

(2)  A  Unisys  80386  Computer  with  80387  Math 
Co-processor  and  the  "C"  Programming  Language. 

(3)  The  Data  Translation  DT2867  frame  grabber  and  frame 
processing  board. 

(4)  The  Data  Translation  DT2878  high  performance  image 
and  signal  processing  board. 

(5)  The  "NeuralWorks  Professional  II/PLUS"  neural 
network  software,  which  provides  a  software-based  artificial 
neural  network. 

An  illustration  of  the  setup  is  found  in  Figure  4 . 
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System  Setup 

The  baseline  setup  for  basic  ranging  functions  placed 
the  cameras  a  variable  distance  apart,  depending  on  the 
probable  distance  of  the  object  to  be  ranged.  For  initial 
calculations,  the  cameras  were  set  up  on  a  stand  which  kept 
them  at  a  four  inch  separation.  An  experiment  to  determine 
the  optimum  distance  between  the  cameras  for  variable  ranges 
was  found  to  be  unnecessary.  In  general,  wider  camera 
separation  and  greater  resolution  of  the  cameras  will 
provide  greater  stereo  ranging  accuracy.  However,  a 
combination  of  wide  camera  separation  and  limited  field  of 
view  can  cause  problems  for  stereo  ranging  and 
identification  of  objects  very  close  to  the  camera  array. 

One  solution  to  the  variable  range/variable  resolution 
problem  adds  a  third  camera  and  allows  the  user  to  switch 
be  »een  camera  pairs.  By  thus  selecting  the  pair,  optimal 
results  may  be  attained  under  varying  conditions. 

The  actual  hardware  custom-built  by  NASA  to  accomplish 
the  docking  solution  would  be  reduced  in  size  to  a  box  even 
smaller  than  the  80386  computer  actually  used  to  conduct 
experimentation.  This  box  would  be  connected  to  the  cameras 
within  the  shuttle  payload  bay  in  much  the  same 
configuration  as  the  current  system  setup.  Some  means  for 
the  Shuttle's  on-board  computer  to  activate  the  docking 
system  would  be  necessary  to  initiate  acquisition  and 
tracking  of  the  space  station.  Once  the  hard-wired 
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algorithms  for  stereo  vision,  object  recognition,  location 
and  orientation  determination,  and  waypoint  and  trajectory 
computation  were  executed,  some  means  of  transferring  the 
calculated  information  to  the  Shuttle's  on-board  computers 
would  also  be  necessary. 


Basic  Ranging  Problem 

In  order  to  allow  the  computer  to  range  an  object,  a 
camera  constant,  in  units  of  image  pixels  per  inch  of 
surface  along  the  imaging  array  at  the  back  of  the  camera, 
had  to  be  determined.  Only  then  could  algorithms  be  written 
to  calculate  the  distance  based  on  the  correspondence  of 
images  from  the  two  cameras.  The  number  of  pixels  the  image 
displaced  from  one  camera  to  another,  combined  with  the 
pixels  per  inch  constant,  enabled  the  computer  to  determine 
the  effective  length  of  the  apparent  image  separation  in 
length  units  rather  than  pixel  units. 

The  pixels  per  inch  of  the  cameras  was  determined  by  a 
basic  experiment  which  used  only  one  of  the  cameras  and 
assumed  that  both  were  reasonably  similar.  The  focal  length 
was  already  known  to  be  7.5  millimeters.  A  target  of  known 
width  and  distance  from  the  camera  was  taken,  and  a  one- 
camera  ranging  equation  was  used  to  determine  how  long  of  a 
stretch  of  the  Charge-Coupled  Display  (CCD)  the  image 
occupied.  This  equation  operated  on  the  premise  of  similar 
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triangles.  The  distance  to  target  and  length  of  target  was 
one  set  of  variables,  and  the  focal  length  and  actual  length 
of  subtended  CCD  was  the  other  (Figure  5) . 

Target  Length  _  Length  of  Subtended  CCD 
Distance  to  Target  Focal  Length 

By  simply  using  a  program  to  count  the  number  of  pixels 
subtended  by  the  object,  the  pixels  per  inch  were  determined 
by: 


Pixels  per  inch  * 


Number  of  Pixels  Subtended  by  Target 
Length  of  Subtended  CCD 


A  rough  estimate  of  the  camera  variable  was  found  by 
repeated  experiments  using  a  four  inch  (10.16  cm)  target  at 
a  distance  of  32  inches  (81.28  cm).  The  target  subtended 
98.25  pixels  in  the  camera  image.  The  value  of  the  constant 
was  found  to  be  approximately  2,655  pixels/inch. 

Stereo  ranging  utilizes  the  mathematical  relationship 
among  focal  length,  distance  to  target,  camera  separation, 
and  image  disparity  (parallax)  of  the  two  cameras.  The 
general  equation  relating  these  four  variables  is  derived 
using  similar  triangles  as  shown  in  Figures  6  and  7.  [6] 


17 


Stereo  Ranging  and  Supporting  Variable  Equations; 


Distance  to  Target  _  Distance  to  Target  +  Focal  Length 
Camel  a  Separation  Length  of  Subtended  CCD 


Length  of  Subtended  CCD  -  Camera  Separation  +  Image  Parallax 


Image  Parallax  =  Number  of  Pixels  Offset  •  Inches  per  pixel 

In  order  to  accomplish  stereo  ranging  with  the  system 
used,  it  was  necessary  to  determine  the  effective  parallax 
between  two  images  of  a  single  object  in  both  cameras. 

First,  the  object  was  isolated  using  image  binarization . 
Binarization  transforms  the  image  from  one  containing  many 
shades  of  gray  to  a  black  and  white  image  based  on  a  certain 
threshold  (gray-level  value) .  This  gray-level  is  determined 
by  observing  the  gradient  in  gray-level  values  between  the 
object  and  its  background.  A  simple  method  for  determining 
the  threshold  is  to  use  the  mean  of  the  gray-levels  found 
for  the  object  and  background.  For  convenience  in  initial 
experimentation,  black  and  white  objects  and  backgrounds 
were  used.  The  large  gray-level  gradients  they  provided 
simplified  the  binarization  process. 

In  calculating  image  parallax  between  the  two  cameras, 
object  centroids  were  chosen  as  corresponding  points.  The 
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centroids  were  determined  using  the  method  of  chain  coding. 

Chain  coding  is  an  algorithm  which  traverses  the  perimeter 

of  a  binarized  object,  thus  allowing  for  the  calculation  of 

its  mass  moments  and  perimeter.  The  object's  centroid  may 

be  determined  from  its  mass  moments.  Once  the  centroid  is 

found,  the  number  of  pixels  of  offset  between  the  two  images 

is  calculated  as  the  horizontal  displacement  of  one  image 

relative  to  the  other.  This  corresponds  to  the  centroid 

column  value  differences  in  both  cameras.  The  distance  to 

the  designated  object  was  then  found  using  the  stereo 

ranging  equation.  For  this  experiment,  only  horizontal 
% 

correspondence  was  used.  Vertical  or  even  diagonal 
correspondence  could  be  easily  used  if  it  was  proven 
necessary  to  accommodate  the  dual  camera  configuration. 

With  the  cameras  in  a  side-by-side  configuration,  only 
horizontal  correspondence  was  necessary. 


Determination  of  Bearing  to  Objects 

Once  stereoscopic  ranging  had  been  accomplished,  the 
next  step  involved  determination  of  bearings  to  target 
objects.  The  azimuth  and  elevation  to  the  target  were 
necessary  to  compute  the  location  of  the  beacon  in  3-D 
space.  The  easiest  method  of  computing  the  offset  of  the 
beacons  from  the  cameras  was  observation  of  the  displacement 
of  the  centroid  of  the  beacon  from  the  centerline  of  the 
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camera.  Unlike  ranging,  which  required  two  cameras,  bearing 
computation  could  be  done  with  just  one. 

For  determination  of  the  centerline  pixels,  the 
experimental  program  simply  took  the  average  of  the  minimum 
and  maximum  row  numbers.  Computation  of  the  centerline 
columns  was  conducted  in  the  same  way.  This  method  assumed 
identical  pixel  dimensions  along  each  row,  and  orientation 
of  the  camera  such  that  the  light  detection  elements  were 
aligned  perfectly  normal  to  the  centerline.  The  centerline 
passed  through  the  middle  of  the  detection  array.  With 
actual  cameras  on  the  shuttle,  checks  will  have  to  be  done 
to  ensure  this  is  the  case. 

Instead  of  computing  the  azimuth  and  elevation,  the 
program  computed  the  distances  along  the  x-  and  y-axes,  the 
range  computed  using  stereoscopic  vision,  the  focal  length, 
and  the  number  of  pixels  offset  from  the  centerline. 

Figures  8  and  9  illustrate  the  relationships  used  to 
determine  bearing.  The  distances  were  computed  according  to 
the  following  formula,  which  was  derived  from  the  stereo 
ranging  equation: 


x  offset  = 


#  Pixels  Offset  •  Inches/ Pixel  •  Distance  to  Target 

Focal  Length 


In  this  fashion,  the  x,  y,  and  z  coordinates  of  each  beacon 
are  determined,  as  illustrated  in  Figure  (10)  .  The  next 
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problem  lies  in  computation  of  the  Roll-Pitch-Yaw 
orientation  of  the  space  station  relative  to  the  space 
shuttle. 

Determination  of  the  Orientation  of  the  Space  Station 

An  initial  orientation  experimentation  was  performed 
using  three  light  targets  marked  in  white  on  a  black 
background.  This  polarity  shift  between  object  and 
background  made  it  necessary  to  incorporate  the  analysis  of 
light  objects  on  dark  backgrounds  in  the  chain-coding 
routine.  The  three  targets  were  positioned  to  form  a 
Cartesian  coordinate  frame,  with  beacon  1  at  the  origin  and 
beacons  0  and  2  along  the  x-  and  y-axes.  The  z-axis  of  this 
frame  coincided  with  the  approach  direction  for  docking. 

This  arrangement  was  chosen  to  allow  complete  determination 
of  the  space  station's  orientation,  as  seen  in  Figure  (11). 
These  three  vectors,  normalized  to  unit  length,  were 
arranged  into  a  3-by-3  rotational  transformation  matrix. 

This  matrix  held  all  of  the  orientation  information 
necessary  to  compute  the  roll,  pitch,  and  yaw  of  the  three- 
beacon  structure  relative  to  the  docking  platform. 

Determination  of  Waypoints  and  Trajectory 

Once  the  orientation  and  coordinates  of  the  space 
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station  are  known  relative  to  the  shuttle,  the  waypoints 
which  the  shuttle  should  pass  through  or  near  on  its  path  to 
the  docking  port  may  be  calculated  (See  Figure  12) .  The 
first  waypoint  is  the  shuttle's  current  location.  The 
second  is  a  point  straight  ahead  of  the  shuttle  at  a 
distance  which  would  allow  it  twice  the  time  necessary  to 
rid  itself  of  any  current  translational  or  rotational 
velocity  (i.e.,  come  to  a  complete  stop).  The  thi’  -«oint 
is  similarly  located  in  front  of  the  space  station  docking 
port,  but  at  a  distance  to  allow  three  times  the  minimum 
deceleration  time  (to  force  a  slow  approach) .  The  fourth 
and  fifth  points  are  at  the  docking  port  itself.  This 
duplication  provides  for  a  complete  stop  in  the  trajectory 
calculations . 

Richard  P.  Paul  [7]  provides  some  simple  equations  to 
accomplish  the  smoothing  of  trajectories  given  the  waypoints 
through  which  a  robotic  end  effector,  or  a  shuttle  in  this 
case,  must  pass.  A  smooth  trajectory  provides  more 
efficient  use  of  energy  since  complete  accelerations  and 
decelerations  at  each  point  are  unnecessary.  The  shuttle 
need  only  pass  near  most  of  the  waypoints. 


Trajectory  Algorithm: 
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acceleration  =  — i_  •  (AC  •  4.  ~  AS) 

2t2  r 

velocity  =  •  (AC  *  -  A B)  •  A  +  A? 


posi  Cion 


( (AC  •  —  -  AS)  •  h  +  2AB]  •  h  +  Initial  position 
T 


These  equations  are  used  with  each  set  of  three  consecutive 
waypoints.  The  position,  velocity,  and  acceleration  refer 
to  each  of  the  x,  y,  and  z  coordinates  and  pitch,  roll,  and 
yaw  orientations  of  the  shuttle.  AB  is  the  difference 
between  the  parameters  at  the  first  and  second  waypoints, 
while  AC  is  the  difference  between  the  second  and  third. 

Tau  (T)  is  the  transition  time  before  and  after  the  central 
waypoint  state  over  which  acceleration  occurs.  T  is  the 
total  time  over  which  each  transition  and  region  of  linear 
motion  occurs.  H  (0  <  h  <  1)  is  a  counter  to  allow 
computation  of  the  position,  velocity,  and  acceleration  at 
any  point  in  the  trajectory  (0  is  the  beginning,  and  1  is 
the  end  of  the  trajectory  leg) .  The  entire  trajectory  over 
five  waypoints  may  be  calculated  using  two  loops  of  the 
described  equations.  Leg  one  would  include  waypoints  one, 
two,  and  three,  and  Leg  two  would  include  waypoints  three, 
four,  and  five.  The  duplication  of  points  four  and  five 
provide  for  a  complete  stop  at  the  docking  port.  [7] 
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Autonation  of  Calibration  and  Ranging  Algorithms: 

Determination  of  the  pixels  per  inch  scale  factor  of 
one  camera  was  accomplished  through  the  use  of  an 
interactive  program  between  user  and  vision  system. 
Variations  on  this  approach,  using  a  computer  "mouse"  as 
input,  could  be  implemented  on  the  deployable  unit.  This 
problem  requires  the  determination  of  the  number  of  pixels 
subtended  by  an  object  of  known  width  at  a  known  range.  The 
edges  of  the  target  can  be  determined  by  finding  the  two 
sharpest  gradients  in  the  selected  region.  The  region  is 
chosen  such  that  these  correspond  to  the  left  and  right 
edges  of  the  object.  For  the  two  cameras  mounted  behind  the 
crew  deck  of  the  shuttle,  an  example  of  a  suitable  object  is 
the  large  vertical  stabilizer  prominently  mounted  in  the 
cameras'  field  of  view.  After  counting  the  pixels,  the 
algorithm  would  have  the  user  enter  the  width  of  the  target 
and  the  distance  to  it,  or  it  could  use  known  values  in  the 
case  of  the  fixed  vertical  stabilizer.  The  pixels  per  inch 
constant  will  generally  remain  unchanged  for  any  given 
camera,  assuming  negligible  expansion  of  camera  imaging 
elements  from  post-launch  heating  of  the  shuttle  orbiter. 

The  incorporation  of  these  automatic  calibration  routines 
mostly  assists  in  the  'portability'  of  this  software  docking 
system  to  include  other  camera  systems. 

Automatic  calibration  of  the  alignment  between  the  two 
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cameras  (only  accounting  for  horizontal  plane  cant)  could 
also  be  accomplished.  This  algorithm  would  allow  the  user 
to  determine  the  limiting  columns  between  which  the 
calibrating  target  is  located,  and  determine  the  edges  of 
the  target  in  each  camera  by  finding  the  two  sharpest 
gradients  in  the  designated  regions. 

Correlation  would  be  used  to  find  corresponding  edges 
in  both  images.  By  using  a  loop  and  a  3,5,  or  7-pixel 
segment  from  one  camera,  preferably  centered  around  an  edge, 
it  could  be  slid  along  the  same  row  of  pixels  from  the  other 
camera.  Based  on  the  user's  prediction  of  the  corresponding 
location,  a  reduced  area  could  be  searched.  The  point  of 
maximum  correlation  would  be  the  location  of  the 
correspondence  point  between  the  two  images.  This 
information,  along  with  the  user-supplied  range  to  target, 
is  enough  to  determine  the  yaw  angle  of  one  camera  relative 
to  the  other. 

Compensation  for  Camera  Irregularities 

Because  of  the  "fishbowl"  effect  caused  by  the 
distortion  from  the  cameras'  lenses,  only  the  central 
portion  of  the  screen  was  used  for  computations.  The  whole 
camera  area  could  be  utilized  once  the  extent  of  the 
"fishbowl"  effect  on  measurements  near  the  edge  of  the 
screen  had  been  determined.  It  would  require  the 
development  of  a  screen  masking  system  or  formula  which 
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would  take  into  account  the  increased  distortion  at  the 
limits  of  the  camera's  view. 


Visual  Beacons 

Current  NASA  approaches  to  its  spacecraft  docking  and 
tracking  problems  utilize  visual  beacons.  Visual  beacons 
must  be  contrasted  against  the  background  of  the  docking 
object  in  order  to  be  easily  located  by  eye.  Visual  beacons 
must  also  provide  necessary  ranging  and  orientation 
information  to  the  approaching  spacecraft,  in  order  to  allow 
the  operator  to  perceive  the  necessary  trajectory  to  achieve 
docking . 

This  research  incorporated  the  development  and  use  of 
visual  beacons  to  simplify  the  tracking  problem.  If  the 
system  were  required  to  identify  the  space  station,  and  home 
in  on  it,  problems  would  be  encountered  once  the  approaching 
spacecraft  closed  within  a  certain  distance  of  the  target. 
The  camera  system  would  not  be  able  to  visualize  the  entire 
object  even  if  it  were  mounted  at  the  extreme  aft  areas  of 
the  spacecraft. 

A  more  logical  approach  involves  using  the  tracking 
system  to  identify  the  space  station  at  longer  distances, 
and  to  identify  the  visual  beacon,  and  home  in  on  it  at 
shorter  distances.  A  possible  criteria  for  the  initiation 
of  the  switch  between  space  station  tracking  and  visual 
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beacon  tracking  would  be  the  percentage  of  the  screen  the 
space  station  filled.  Once  the  station  filled  half  the 
screen/  the  system  would  start  searching  the  image  for  the 
visual  beacon. 

To  facilitate  the  search  for  and  use  of  the  visual 
beacon/  the  design  of  the  beacon  itself  is  important.  There 
should  be  great  contrast  between  the  beacon  and  its 
background  (the  space  station) .  Beacons  could  be  of  unique 
and  simple  geometric  design,  to  allow  the  vision  system  to 
identify  it  with  simpler  pattern  recognition  algorithms. 

The  unique  design  and  arrangement  of  the  beacons  must  also 
convey  all  necessary  range  and  orientation  information  in 
detail  to  the  tracking  system. 

The  current  visual  beacon  used  by  NASA  is  three- 
dimensional.  Simply  described,  it  is  a  pole  projecting  from 
a  circular  background.  The  tip  of  the  pole  is  one  color, 
while  the  background  is  another.  If  the  approaching 
spacecraft  is  not  coming  in  on  a  straight  trajectory,  the 
tip  of  the  pole  will  appear  offset  within  the  circular 
background,  indicating  the  direction  and  degree  of 
correction  necessary  to  restore  the  proper  approach  path. 
Range  is  indicated  by  the  apparent  size  of  the  circular 
background,  based  on  a  scale  built  into  the  video  systems  of 
the  approaching  spacecraft . 

The  stereoscopic  nature  of  the  proposed  system 
eliminates  the  need  for  prior  knowledge  of  the  dimensions  of 


the  beacon.  However,  thus  far,  knowledge  of  the  relative 
orientation  of  the  two  objects  still  requires  some  type  of 
three-dimensional  beacon  structure.  A  series  of  two- 
dimensicnal  beacons  would  be  preferable,  as  they  could  be 
painted  or  placed  more  conveniently  at  any  docking  location. 

Part  of  the  experimentation  in  this  research  involved 
the  development  and  evaluation  of  scaled  versions  of  two- 
dimensional  visual  beacons. 

Holographic  Beacons 

A  fascinating  possibility  is  that  of  a  holographic 
visual  beacon.  Given  the  advances  in  holography  and  LED 
lasers,  it  is  feasible  to  have  a  flat  plate  of  holographic 
film  laid  over  a  bed  of  Light-Emitting  Diode  (LED)  lasers. 

As  spacecraft  approached  the  docking  location,  the  visual 
beacon  could  be  turned  on.  The  holographic  beacon  could 
possibly  contain  the  image  of  a  ten-foot  pole  projecting 
into  or  out  of  the  spacecraft.  A  beacon  of  this  nature 
would  provide  very  accurate  information  on  the  orientation 
of  the  spacecraft  while  taking  up  minimal  space. 

Discussion  of  Pattern  Recognition  Approach 

Among  the  attributes  of  a  desirable  visual  beacon  would 
be  distinctive  characteristics  which  would  make  it  easily 
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distinguishable  to  a  machine  vision  system.  These  include 
different  orders  of  moments  about  the  centroid  of  the  image 
of  the  beacon.  If  the  size  of  the  beacon  were  known,  then 
the  observed  range  and  size  of  the  object  could  be 
incorporated  into  the  pattern  recognition  criterion.  After 
using  histograms,  binarization,  and  edge-detection 
techniques  to  isolate  high-contrast  objects  within  the  field 
of  vision,  the  system  would  analyze  each  to  determine 
whether  its  moment  characteristics  matched  those  of  the 
desired  visual  beacon. 

If  the  average  albedo  (image  lightness  or  darkness)  of 
the  space  station  were  known,  which  it  would  probably  be, 
then  the  system  could  normalize  the  image  operated  upon 
based  on  that  value.  Assuming  also  that  the  albedo  of  the 
visual  beacon  were  known  relative  to  the  space  station,  one 
possible  approach  to  quickly  identify  the  beacon  would  be  to 
choose  its  color  whereby  reflectivity  and  location  in  the 
spectrum  were  unique. 

This  approach  is  hampered  by  a  fundamental  difficulty: 
the  variable  lighting  of  the  sun.  On  the  day  side  of  the 
earth,  reflection  would  require  dampening  filters.  On  the 
night  side,  searchlights  might  be  necessary  to  provide 
enough  information  to  the  vision  systems.  An  alternative  is 
the  use  of  infrared  vision  systems,  active  or  passive,  to 


find  the  beacon.  However,  this  approach  will  not  be  covered 
by  the  scope  of  this  project. 

Neural  Networks  for  Object  Recognition 

Because  of  the  uncertainties  involved  in  the 
correlation  of  unknown  object  parameters  to  those  of  the 
beacon,  an  artificial  neural  network  could  greatly  speed  the 
identification  process.  An  artificial  neural  network  is, 
according  to  Robert  Hecht -Nielsen,  "...a  computing  system 
made  up  of  a  number  of  simple,  highly  interconnected 
processing  elements,  which  processes  information  by  its 
dynamic  state  response  to  external  inputs."  [8]  An 
illustration  of  an  individual  artificial  neuron  model  is 
found  in  Figure  (13) .  Using  the  various  momenta  of  the 
objects  as  input  parameters,  a  trained  artificial  neural 
network  would  output  the  degree  to  which  the  unknown  object 
matched  the  characteristics  of  the  known  beacon. 

This  project  investigated  the  effectiveness  of  varying 
numbers  of  parameters  in  a  neural  network  in  identifying  a 
visual  beacon  from  among  a  pool  of  unknown  objects.  Other 
than  moments,  neural  networks  could  also  search  for  other 
patterns,  possibly  allowing  the  use  of  distinctive, 
intricate  designs  as  the  framework  of  the  beacon.  Software 
based  neural  networks  are  available  for  use  on  personal 
computers,  and  can  be  incorporated  into  the  C  routines 
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already  being  used  to  interact  with  the  frame-grabbing  and 
frame-processing  boards. 


Identification  of  Beacons  using  Neural  Networks 

Differentiation  among  the  three  beacons  <0,  1,  and  2) 
was  necessary  for  proper  determination  of  the  orientation  of 
the  station.  The  preferred  method  of  distinguishing  objects 
was  an  artificial  neural  network  approach.  Artificial 
neural  networks  using  four  of  invariant  moments  of  Hu  [9] 
and  compactness  (perimeter2/area)  as  inputs  proved  to  be  an 
effective  way  of  classifying  the  digits  zero,  one,  two,  and 
three  from  inclinations  of  up  to  60  degrees  from  horizontal. 
A  simple  demonstration  of  this  capability  was  performed  on  a 
separate  camera  system,  and  the  resulting  neural  network 
subroutine  was  incorporated  into  the  trajectory-computation 
program.  Training  of  the  neural  network  was  accomplished 
using  simple  duplicates  of  the  actual  beacons.  These 
targets  were  photographed  repeatedly  using  a  camera  system. 
Their  invariant  moments  and  compactness  were  calculated,  and 
the  value  of  the  digits  broken  down  into  binary 
representation  by  two  bits  of  0  and  1.  The  resulting  data 
was  saved  to  a  disk  file  from  which  the  training  program 
retrieved  the  information.  Therefore,  a  five  input,  two 
output  neural  network  was  used.  One  hidden  layer  neuron 
proved  to  be  sufficient  in  this  case  to  distinguish  between 
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these  simple  digits.  Figure  (14)  illustrates  a  schematic  of 
the  applied  artificial  neural  network. 

This  approach  assumed  that  beacons  similar  to  the  ones 
used  in  experimentation  would  have  to  be  placed  onto  the 
space  station/  either  before  launch  or  during  an 
extra-vehicular  activity  afterwards.  Given  that  the  Mir 
space  station  is  already  in  place,  and  that  extra-vehicular 
activities  to  place  such  beacons  on  the  station  would  be 
costly  and  probably  inaccurate  in  their  results,  an 
alternative  method  of  fixing  the  station's  coordinates  and 
orientation  was  found  to  be  necessary.  The  three-beacon 
approach  was  still  feasible.  However,  instead  of 
subsequently  placed  targets  specially  designed  for  docking 
operations,  existing  structures  on  the  station  were  to  be 
used. 

The  same  neural  network  could  have  been  trained  to 
recognize  individual  objects  integral  to  the  construction  of 
the  space  station.  Examples  include  the  solar  arrays,  an 
antenna,  or  even  the  name  of  the  vehicle,  if  it  were 
emblazoned  prominently  on  the  side  of  the  spacecraft. 

Given  that  the  computer  had  prior  information  fixing  the 
locations  of  these  identified  objects  in  a  given  three- 
dimensional  pattern  on  the  station,  the  orientation  of  the 
station  could  be  calculated  in  the  same  method  used  to 
determine  orientation  of  the  previously  discussed  three- 


beacon  structure. 
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Neural  Network  Trainers  and  Expert  Systems 

Further  flexibility  of  this  system  could  be 
accomplished  by  combining  the  artificial  neural  network 
training  scheme  with  an  artificial  intelligence  "expert 
system".  Combined  with  a  simple  database,  this  format  would 
have  a  picture  inventory  of  prominent  objects  to  be  found  on 
any  number  of  space  vehicles.  With  input  from  what  aspect 
and  which  vehicle  the  shuttle  was  to  rendezvous,  the 
autonomous  docking  system  could  choose  objects  for  use  as 
beacons,  formulate  their  two-dimensional  images  as 
determined  from  their  three-dimensional  computer  models,  and 
compute  their  predicted  characteristics. 


Horizons,  Vision,  and  Conclusion 

This  research  covers  many  of  the  fundamentals  of 
machine  vision.  Stereo  vision  and  the  location  and  tracking 
of  an  object  in  three-dimensional  space  was  accomplished. 

Use  of  three  of  these  beacons  helped  accomplish 
determination  of  the  orientation  of  a  larger  object,  such  as 
the  simulated  space  station.  The  success  of  artificial 
neural  networks  in  distinguishing  between  these  beacons  (in 
a  Cartesian  coordinate  frame)  was  necessary  to  calculate  the 
normal  point  projecting  from  the  docking  port  and  further 
automate  the  docking  process.  From  this  information. 
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waypoints  and  trajectories  were  calculated  to  the  station. 
These  valuable  sensor  technologies,  coupled  with  other 
developing  technologies  such  as  artificial  intelligence, 
will  permit  practical  automation  of  space  operations  on  a 
large  scale. 

With  the  incredible  potential  that  automation 
technologies  offer,  scientists  will  definitely  proceed  with 
their  studies  in  these  fields.  Machine  vision  is  one  of  the 
major  keys  to  the  fascinating  fields  of  robotics  and 
automation. 
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Figure  1:  Shuttle  (with  Docking  Adapter)  about  to  rendezvous  with  futuristic  space  station. 

(Taken  from  Space  Station,  1988,  K.  Alexander) 


TABLE  If.  THE  MR  SPACE  STATION 


Hardware  Configuration 


Figure  4:  Hardware  Configuration  and  Basic  System  Setup 


Width  of  Target 


Figure  5:  Relationship  used  to  determine  Pixels  per  inch 


Left  Camera  View 


Stereo  Vision  -  Camera  Setup,  Views,  and  Variables 


#■ 


Figure  8:  Bearing  Determination  Diagram  and  Camera  View 


X  Offset 


Figure  9:  Bearing  Determination  Variables  and  Equations 


Location  of  Three  Points 


Figure  10:  Graphic  Model  Depicting  3-D  Loactions  of  Objects 


Determination  of  Normal  Line  through  Spaoe  Station 


Figure  12:  Depiction  of  Waypoint  and  Smooth  Trajectory  Determination 


Mathematical  Model  of  An 


Mathematical  Model  of  Artificial  Neuron  Operation 


5  Input,  2  Output,  Back-Propagation  Artificial  Neural  Network 

1  Hidden  Layer  Neuron 
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Figure  1 4:  Model  of  Neural  Network  Used  in  Experiment 
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r - 7 

r  Mmample  VMon  Routine  written  by  Wetted  M.  Hau,  WDN  1/C  1989  7 
r  Thta  program  mm  ttw  DT2M7  Frame  Grabbing  board  In  eoniunetion  with  7 
/*  a  pair  of  Panaaonle  atbtl  oamaraa  and  analog  lo  digital  converters.  7 
/*  Pswsral  reudnaa  warn  writtsn  wrih  Mm  aaalatanoa  of  Protaaaor  7 

/*  WNan  daawnt  of  the  Weapons  and  Systsma  Engineering  Department  7 

r - v 

r  Hun  on  bbcroooft  C  TjD,  ualng  POPPP  prooaaaor.  largo  memory  modal.  V 
r  and  10SM  ataok  opdona.  Indude  CEUBJJB,  DTIFUSUB,  QRAPtbCSJJB  •/ 

/*  and  UPb7iJB  A  the  «e  laL  7 


dbialMda  «oonloJt>  r  lor  hbM  7 


ftnduda  donUa  r  raqulrad  for  tPcroaoft's  opan/doaa  7 

dtndude  <ayaaypao.il> 


r  raqulrad  lor  pdntf  •/ 


Dnduda  tfnahJb 

dbiduda  «dtitypJt>  r  Pla  of  DT  apacifle  typaa  •/ 

fHndudo  ■c.-VdtNncUidaUoctUi"  f  WCTL  stuff  7 

Indudo  TcfldtNnrbirttMocitsxtjr 
dbtdude  «omd_angJt»  r  oommand  angina  attiff  7 

dinoludo  «dt2*67Vsg_numJt»  r  raglotar  numbara  7 
dtndude  «dt2S67PbS7Ji>  /*  dt2067  library  Interface  functions  7 
dbiduda  ddjmoJt*  f  naadad  by  lOCTL  oommand  o  7 

dbiduda  «stringJt»  r  for  chain  coda  string  oparatlona  7 


r  mouse  routlna  Indudes  7 

dtndude  «dosJt>  r  tor  lirtMO.  AtdbxO  7 

dbiduda  "mouaaJi*  r  for  "struct  cursor  struct"  7 


struct  objsct  { 
bit  rear, 
bit  ool; 
boat  xbar; 


PW[51; 


r  Ala  atnactura  holds  Aa  Information  for  7 

r  objects  analyzad  by  Aa  ohaln_eoda  function 

r  row  and  cot  daaignata  Aa  location  of  Aa  */ 

/*  starting  pixel  bom  which  chaln_ooda  operates.*/ 
r  xbar  and  ybar  daaignota  Aa  oantrold,  phl(5]  7 
a;  /*  daolgnatoa  Aa  first  4  Invariant  monisms,  and  7 

/*  eompaetnsss  Mums  Aa  compactnass  of  Aa 
/*  object  analyzod 


7 


7 


struct  haaoan  ( 

Poat  xfoarf2],ybarf2J;  r  Comoro  0  and  1  oantrokJs  of  baaoon  7 

boat  adlstydlsudlst;  r  oartasian  ooordlnatao  of  baaoon  7 

): 


struct  waypoint  { 
float  matrixt4]l4]; 
boat  xdlst,ydisUdlst; 
boat  pHeh.ysw.roU; 

); 


/*  homogonaous  transform  matrix  at  this  7 
/*  waypoint  also,  Aa  coordinates  and  7 
r  orientation  of  As  ahuttla  hors.  7 


struct  trajectory  { 

boat  time;  f  individual  trajectory  tints,  position,  7 

boot  xdlst{Zl,ydlat[2J,tdlft[2];  r  velocity,  and  accdaratlon  at  7 

boat  pltch(2J,yawf2J/o<l[2];  r  Ala  point  in  Aa  trajectory.  7 
}; 


struct  delta  { 


/*  for  purposes  of  trajectory  smooAIng,  7 
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■oat  nUatjrdtoUdlst;  r  flte  MM  wtd  MW  dMerances  m  V 
tat  phoh,yew,foa;  /•  position  and  orientation  taaaaa  V 

struct  point)  (  /•  generic  structure  tormat  ter  s  •/ 

Int  matrtxgl;  /•  2-poM  vector  */ 

); 

•truer  point)  {  r  generic  atruetwe  tonaat  tor  a  */ 

•oat  weirttpi;  r  S-poM  motor  */ 

>; 

struct  homog  (  /*  ganarte  structure  format  tor  a  */ 

tat  a>alrtKtH»l;  /*  *-by4  homogeneous  Matrix  V 

h 


abort  Int 
M 


M 

M 

Moot 

tat 

mt 


r  continue  Rog,  0  to  rapaet  V 
r  abort  Integer  eotor  Max  */ 

potartty,  r  flag  to  Mtaato  bacfcground/ohjsct  ooMraat  enaction  t 
r  eountsr  tor  cycling  through  hoocona  */ 
r  counter  tor  cycling  through  waypoints  */ 
ooontarf2];  /*  oantotUne  oohimno  lor  cawioroa  0  and  1  V 

luanwpi;  r  oantarUno  rows  tor  oamaroa  0  and  1  */ 

r  number  of  class  heaenn  la  raoognizad  os  •/ 


r  tor  computation  of  die  ortantailon  ol  a  three  hsannn  structure  */ 
icat  xxjr/Mi  /*  *  unit  vector  •/  r  waypoint  homogeneous  matrix  •/ 

tat  yxjry.yz;  /*  y  unit  sector  v  /*  rotation  values  */ 

tat  zxjy,zz;  f  z  unit  vector  */ 

tat  xd.yd.zd;  /*  ’unit1  vector  magnttudea  tor  scaling  purposae  */ 

tat  tacc,  tou.  ft,  dag,  vmax,  distance,  tvmax;  /*  tacc  */ 

r  tor  transformation  between  coordinate  aystsma  -  normal  and  world  */ 

float  vectorl  [4];  /*  Input  vector  at  change  In  distanoe  along  normal  */ 

float  vactor2(4];  /*  output  vaeior  el  actual  iflatanoaa  along  axaa  */ 

float  vtaw_poaition[)]; 


/•  neural  network  classiflcatian  constants  */ 


float 

datajnf)]; 

/*  S  Input  values  tor  network  */ 

float 

data_out{2]; 

/•  2  output  values  tor  network  */ 

float 

rasult;  /*  weighted  sum  of  outputs  tor  classiflcatian  */ 

/*  structure  olloeations  */ 

struct  otyaet 

ohjeett; 

/*  object  sont  to  ehaln.codo  •/ 

struct  hsooon 

beaeons()]; 

/•  )  heaonns  identified  and  analyzad ' 

struct  weypoint  waypolnts(S]; 

/*  S  waypoints  computed  tor  docking  •/ 

struct  tia|ectory 

tra|ectories(40]; 

/*  40  trajectory  points  analyzed*/ 

struct  delta 

del  tab,  deltac,  a; 

/*  dlftoranoo  computation  between  waypoints  */ 

/*  tor  draw_afl_flgursa  and  graphics  routines  •/ 

struct  point) 

pts(4];  1*  tor  drawing  coordinate  axes  •/ 

struct  point) 

P3a; 

struct  point) 

p»; 

struct  point2 

P3a; 

struct  pofnt2 

pat; 

struct  homog 

AARJ; 

Int 

maxX^naxY; 

r  max  ptxaf  count  for  graphics  mods 

•/ 

unsigned  char 

*axls_text(4); 

r  tor  marking  Idantity  of  axes  */ 

unsigned  char 

flllmaaklB]; 

/*  tor  later  fining  In  of  polygons 

•/ 

float 

aspect;  f*  aspect  ratio  of  monitor  */ 

float 

scale; 

r  scaling  of  Imago 

•/ 

float 

dial; 

/*  distance  of  ‘eya’  from  origin  */ 

iiliiiij  iiii 


54 


/*( 

Ml 

•MMpoM2 


MfllMPl:  I*  VOOlOf  «t 

to  plotting  points  •/ 


/•  otructuro  fonoot  uootlo  by  bonotono_pobit  1 
pMtiptot(40];  l*  output  ton— t  ol  tro)ootory  from  tmnoform _pobttf 


r  tor  nkhioo  raudnoo 


1 

01090; 


tor  dotonolnlng  whan  to  ctiongo  ouroor  ohopo;  •/ 

MAX.s,  tOXjr.  f  numtonr  ol  pints  In  ooalt 

b_topa79,  b_bottoata7S,  b  MtolOO,  b  rightolOO; 


bit  status,  numjbuttons; 
bn 


bit_osg,  lnt_off,  lnt_mosk; 


r 


Information  gathering  rautinoo  for  otorao  vision  •/ 


toho_plK(bignUnt); 
tote_tom_pbi  (Ini); 
flot_poliM(lnt  \lnt  VntlW); 


chotai_eods<stiuet  obfoct  *objacti,  bool  alpha^m  thraahold,im  button  M); 


iphtoaMra|actory  display  reuttnoo  •/ 
hiHOs 

oomputojilMf jrono«orai(llooi  thoto,  Hoot  gamma,  boot  motrtxI3H3D; 
oomputo_Rf>Yjranotorm(float  yaw,  boot  pitch,  boot  rob,  boot  matrixf31PD; 
oomp«ito_AzEI_trano«onn(b  or,  float  ol,  boot  mo«rtx(3]I3D; 
tronoform_polnt(lnt  tr.  boot  pi  [3],  bit  pO[2D; 

*ow_oll_bgurooO; 

dloploy_tro|«ctoryO; 


/*  nouml  notwarb  ctosoMcotion  rouUno*/ 
voM  olooolfyO; 

r—  —  11  -  - — — •/ 

Int  moInO 

{ 


*nt  IJJu  r  loop  countoro  V 

/*  mouoo  routine  vortabtoo  V 


Int 

r  okt  mouoo  position  */ 

Int 

*i.yi;  /* 

now  mouoo  position  */ 

Int 

pi; 

r  button  prooo/roloooo  Indiootor  V 

Int 

P2.  p3;  /* 

button  proof  location  •/ 

Int 

p«.ps;  r 

button  ntloooo  location  */ 

Int 

motatuc;  /* 

mouoo  ototuo  Indiootor  */ 

Int 

"00; 

r  bog  for  mouoo  button  proas  •/ 

Int 

okJtagoO; 

/*  bag  for  rastoraUon  of  Imogo  */ 

r  got_pobit  to  choln.eodo  tronoport  vortbbtoo  */ 
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tat  ooimaMownuat;  r  values  ebtalnsd  hem  arauae  routine  V 

P  monitor  euraor  vartabiso  •/ 

M  row,  ooatl,  ootrtl;  r  row,  Ml  oofctwn,  right  column  numbers  V 

M  rowupLrowdnLaol;  P  cel,  top  raw,  bottom  raw  numbers  •/ 

/*  ornimni  and  LUT  Information  tor  DT2M7  heard  V 

M  bud,  camera;  P  buflarfeamara  numbers  V 

M  H.WMI  A*  OT3M7  board  handle  and  MM  Indicator  •/ 

M  wueah,  threshold;  r  Mnartxation  mid  comparison  thresholds  V 

Int  cemOnum.  oamlnum;  P  mimbar  of  pixels  In  camera  0,1  One  arrays  •/ 

Ini  camaraOfTat];  A*  camera  0  Kne  array  •/ 

M  oamaraipai};  /■  eaanta  1  tea  array  */ 

A»  variables  far  dlatan  as  computation  uoing  edge  data  Wien  along  roam  -  not  tread  *A 

M  found,  pixbagln;  A*  edge  tound  flag,  aarting  pixel  value  */ 

Int  adgaooh^adgaceil;  /*  obfeet  edge  in  camera  0,1  V 


A*  staraotooplc  ranging  constants  •/ 


A*  acdUng  taetar  In 
A*  tooai  length  In 
P 


In  mini 


rdnioiMiilA  ranolna  mriablM  •/ 
Int  pixdtft-. 


P  dtfteranoe  In  pixaia  •/ 

A*  para  Hat  between  two 
P  computed  distance  tram 


•A 

•A 


ujeng 

XY_rgn_huf 


rodtZSOl,  graanpss],  btue{296]; 
rgn,  rgri2,  blankjine,  b4ank_Una2; 


P  START  OF  MAM  PROGRAM  */ 


P  act  video  mode  to  1 6-color  VGA  *A 
_aatvidaomoda(  JARES16COLOR  ); 

jdaaneraanfjacLEARSCREEN); 

/•open  the  DT2067  board  */ 

If  ( (tdaopennmaSOT,  0_R0WR))em1  )  { 

prlnttfEnor  opening  DT-2867  device.  Exiting  _Vi\n"); 
axMO); 

) 


dt67_dlap  (fd,  OTS7_ON);  P  turns  tho  davloa  on  •/ 

dtBTJnp  syne  (fd,  DT67_8YNC_CUH);  P  use  current  channel  as  syne  sou  roe  •/ 

dtOTJnpjiming  (fd,  DT67JEXTERNAL);  P  timing  aouroe  Is  external  to  board  •/ 

r  initial  configuration  of  Input  Look  Up  Tables  */ 
tor(laO;i«2S6,-i*»)  A*  Sat  LUT  values  */ 

greanPM;  P  Normal  picture  */ 

status  a  dt67  Inp  kit  (fd,  DT87_WWTE,  DTB7LUT,  DT67_LUT0, 0, 256,  groan); 

If  (status  Is  EJIORMAL) 

prtntfT Abnormal  LUT  operation  _Vt"); 

P  Un-oommant  this  function  for  demonstration  of  two-camera  parallax  */ 
r  taka_two_plx(fd);  */ 


P  This  section  simply  takas  a  single  picture  with  camera  0,  allows  the  user 
to  designate  an  object,  and  find  Its  centroid  using  chain_eode  V 
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r  Taha  Ml  picture  using  samara  0  •/ 
r  prfatfPnPreparfng  lo  at*  picture  with  cmwn  OQ; 


MM; 

gaf_poktl(teaktua^Biuamua^bufMd); 

pttntfpn(ool  %U/aw  %M)  la  tha  pcaktcn  aalachd  "jodmmjtm mm)n 

BfHaottjoaaaoamum; 

nhjtnH  oolwnnlm— ; 


_ooda(bab)acttM>te.Stra«>oid,butf.fd); 
piMPnlM^lUl)  la  tha  (rew^oei)  of  tha  oantroid',,o6iact2-xbar,objact2.ybar>; 


alphmriJ);  r  ommn  pixais  are  square  V 

threaholdalM;  /*  aUddta  grey  vatua  to  allow  detection  o<  0  and  23*  */ 


r  determina  camarUa  pixels  ol  cameras  •/ 
fOrfoamarmdhcantare  a»l;camaro»+)  { 

reent*rtcamere)s2S5.S;  r  512/2-J  */ 

eoantarfoaniara)s3t&5;  /*  7W2-J  •/ 

> 


r  TMa  loop  cycles  through  both  cameras,  V 

/*  taking  pictures  of  and  analyzing  aaeh  baaoon  •/ 

«or(oamatreO,oamare  «1;camafo»a)  { 

bufheemero;  f  buffer  corresponding  to  oomara  */ 

tate_pix(aaiiiarajbuff,fd);  r  taka  picture  and  thraahoid  V 

r  TNa  loop  cycles  through  al  thraa  baacotta  In  tha  ourranl  bnaga  */ 
for(nunihaannnan;numheaooi»c*2;numbsacioni  i)  { 

prfntipntnCamara  *%3d,  Buffar  S%3d,  Bastion  Kid  ".camarm^aamare^mm  baaoon ); 
prtnttpnPtaaia  aalact  a  point  to  tha  Ml  of  baaoon  %3d  '.num  baaoon); 
get_point(aooinum>*roamum£ufflfd);  /*  gat  atari  roar  and  ookimn  bom  mouaa  V 

ob|sct2joamroamum;  r  give  obfsett  atructure  mouaa  Inputa  */ 

obtact2.ooiaooinum; 

potarttyaO;  r  black  on  arhlta  potartty  */ 

chaln_poda(*obfact2jalpliajhroahcid,buf^fd);  r  partem  chain  ooda  on  baaoon  V 
baaconatnumbaacon].xibartcamerefcob)acff.xt)er;  /*  ghra  baaocna  atructure  oantroid  valuaa  */ 
baaoons(numboaeon].ybar(eaniera]sob|act2.ybar; 

) 

> 

r  Baaoon  Identi  flection  routina  using  naurei  nonaorks  */ 
r  taka  picture  artth  oamara  0  */ 

buff  a  camera; 

taka jii(oamaratbuff  ,fd); 

fOfQaOOoBdaaH 

prtntfTCamare  *%3d,  Buffar  «%3d,  Ob)act  «%3d  ’.camera^amaraj); 
prtntfpn  Please  aalact  a  point  to  tha  Ml  of  object  %3d  ’J); 

r  mouaa  Input  to  loft  of  graphical  designator  •/ 
gat_polntilcolnumlArawmirnlbuff,fd); 
ob)act2jowaroamum; 
objoctZoolscolnum; 

r  chain  coding  of  arhita  on  black  obfact  V 
poiarttysl; 

chain_ooda(&ob)act2^lpha4hreshotdlbuff,fd); 


n  ■»  i  -* 


r  returned  dwainn  tor  um  by  neural  natwoifc  7 


kr(MMiM) 

pctaMpnW^toto_ln(ID: 


etooaMyft 

prtnlipn  Neural  Nat  Outout  «1  >  %M,  Output  ®  *  W,dato_oult014oto_oultl  D; 

r  Interpretation  of  nomork  output  •/ 
reautte2*data_out{0]4daia_outll  ]; 

H  (ioouM  rs.I) 


N  ((rooutuJ)  U  (raouhoiU)) 
atoaonumbsml; 

M  ((resutt>l.S)  M  (msuftasU)) 

H  ((resutt>ZS)  U  (lasuhoSJ)) 


etosanumbamlOO; 

prtntfpnOtofact  Belongs  t>  CtaM  Number  USd’Volasanumbor); 

I 


CaHbratlon  of  eomora  oantsrltoe  pinto,  pitch,  ymw.  roll?  •/ 
knowledge  of  tho  oonun  tom  optfae  would  help  Inemmo  aooumoy  7 

Coord  Inoto  Computation  Routine  7 
lop  1:  determination  of  heaonn  dlotonooo  from  comomo  7 
seal  fact  a  (1  jO/2655 J))*25J«;  r  tooling  factor  In  mlWmotoro  par  pixel  7 

tocotong  a  7JS;  /*  focal  length  In  mlWmalaro  */ 

aamaep  a  4jQ*2&4;  /■  oomero  oeporatton  In  mlMmetero  7 

tetfnumboooonaOpmmheooon  re?;nunibaaoon  1 1)  { 

ptxdlff  a  htaronatnumbsacon).ybartO]  hsaonnstnumbaaoonl.ybat|1}; 
prfntipnlnPlxei  diftoranoo  between  Images  a  %4d  ptxeis",pixdlff); 
parallax  a  ptxdlff  *  scalfact;  r  parallax  In  mWImotara  7 

diatanoa  a  tooatong  *  e amaap  /  para lax;  /*  dtotanoo  bam  cameras  In  mMimatars  */ 

printfQn%8Tj>ermMax); 

printfpnOI stance  from  camaraa  In  mBIrn  stars  a  %dT,dlstanoe); 
prtnttpn  Distance  from  cameras  In  Inches  a  %6f",dlstanea/254); 

r  z  dtotanoo  to  mi  Hi  motors  -  straight  out  bom  shuttle  7 
bsarcnt{numhaaoon).rtltt  a  diatanoa; 

r  bom  tho  camera's  view,  xdlst  Is  positive  to  the  top  along  rows 

ydist  Is  positive  to  tho  right  along  columns  7 

r  datorml nation  of  ’bearing'  of  bastions  bom  camera  setup  7 
r  determine  xdlst  and  ydist  bom  zdtst,  roan  tar,  coon  tor,  xbor,  ybar  7 
r  using  only  camera  o,  and  figuring  to  tho  proper  ydist  compensation  7 

plxdlfCaroan  tort  camera]  beaoonafnumbaaoon^  xbarjcsmatal; 

besoonaCnumbeaoonptolstobasoona|numbasoon]jdHt^tlxdHracaltoetftocstong; 

plxdlffaboaoona{numbaaoon|.ybartcamara>ooa»»iet|camata]; 

baaoonatnumbaaoonl.ydl«tr(l>oacnns[numbaaoonlJdltrplxdHracalfact/tooalangHoamaap<2); 

prtntffto  xdlsta%Sf,  ydlsta%Sf,  zdtsta%0f  * 

^aooowstnumbaaconpMBatJtoaoona|numbaaoonl.ydtot 

.baaconatnumbaaoonlJrtlat); 
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) 


r  norutataa  uaolora  x  (M|,  y  (1-3)  and  mho  croao  product  to  */ 

r  tod  vaotor  along  normal.  Taka  tho  negative  of  Ms  vector  to  */ 

r  gat  tha  x  vector.  Ptaoo  Him*  Hum  vaetora  into  a  *•> 

/*  mIihWImi  Matrix.  Augment  M  wtth  a  poalMoti  tiiair  to  */ 

r  gat  a  4M  tmnafonu  matrix.  Note;  Mia  la  a  tari  handed  •/ 

r  eaoirilnaM  frame 


I*  x  unit  vector  •/ 

/*  compute  Mractonal  dtfloronooo  •/ 

Ky«bMoooi(0).y(SsM>Moonc{l  lytflst; 
xxdbeaoons(01idte*baaooM(l)j*M; 

/•  com  putt  total  magnitude  of  vector  V 
■taaqn»w»ntf)  tpowOqtf>«paw«nj»; 

r  nonnaUza  currant  vector  Into  unit  vector  V 

xxzxx/xd; 

xyexy/Xd; 


/•  y  unit  vaotor  */ 

r  oomputa  in  aaina  mannar  aa  x  unit  vector  */ 
yaabtonmp)  wrtltt  hta non>[1]  irilit. 
yyeboaoonap].ydfot  boacona[ll.ydiot; 
y^banaattaBUdM  boooon^ljjdiot; 
ydsaqrt(pow(yx^>*pow(yy,2>+pow(yz^)); 

iwiotfyri; 

ywfr'yi; 

yznyz^iri; 

/*x  unit  vector*/ 

/*  uaa  oroaa  product  of  x  and  y.  than  uaa  tamo  mathod  •/ 

«x-xy*yx-xrvy; 

xy*xx*yx-xx*yz; 

zz>xx*yy'xy*yx; 

xdaeqrt(pow(zx3)*pov4zy£)«pow(zz3)); 

zxazxAed; 

<IWM; 

noxx/u; 


vmaxz25;  f  maximum  velocity  la  SO  mm/aac  V 

taooaiO;  /*  minimum  time  of  acoeitratton/dooaiarailon  la  10  aaoonda  7 


/*  formation  of  flrtai  waypoint  matrix  -  docking  point  */ 
r  oriantalion  matrix  7 
uwypotntaftpnatrtxPHOIaxx; 

«raypokitsf4pTiatrtx{1  g0]*xy ; 

waypotnu{4pnatrtx(2pixxz^ 

amypoIntaHlJaatrtxtOllloyx; 

waypotntof4]Jnatrix(1  J1  Jayy ; 

waypoint»{4Jjnatrtx[2U1)»yz; 

«raypoint»(4lJTvatrtx[0p)=zx; 

waypoint»14)jnatrtx{1  ][2)=ly; 

waypointa(4]jnatrtx(2K2)sa; 

r  bottom  row  and  acaia  factor  7 

«raypo)nt*{4]jnatrtx[3][0)z0; 

«raypointa]4)jTiatrtx|3]1 1  )*0; 
«rajrpoim*{4pnatrtx[3][2)z0; 
araypoinU{4pnotrlx[3p)>1  ; 

/*  poaitton  augmentation  vactor  7 
waypolntoHpwtrtxtOH3}aboacona{1}jcdiat; 
waypolnta{4)jnatrtx(1  p]*beaoons(1].ydlat; 


w^DtntaMlJBBhUppfchaBOonBtlVJdtat; 


r  Duphcats  Mb  poM  on  waypoint  baton  to  atop  at  docking  V 
tBl(hDj«l^H)  { 

NWi»4ihH 

vrayaolntamnairtxfn|TkwraypBlntaf41jmatrtzfnn]: 

> 

> 

r  DupBeete  this  point  and  subtract  v*tau  from  the  z-axie  */ 
r  this  wii  aBow  a  trajectory  straight  Into  the  docking  port  */ 

«BKhOd^u«>{ 

*MQ*0**i~)  { 

wevooimircUnoli  UJHnUwavDotntsnireatrtxfllfri: 

> 

) 

/•  oomputa  aoraponants  el  nagattva  normal  as  waypoint  2  and  add  hi  •/ 
va  atari  [OfrO; 

VNMrtpHh 

twamqrfMairtaaB;  r  this  dtatanca  out  ban  Via  station  •/ 

wotonpH; 
la KMMIh)| 

MIdHMHi 

vaeto(2fl)Bvaetor3II]«vraypoints{2)jnMrix[nO]*>ractor1(D; 

) 

> 

MlaOMi**) 

traypatntappaatrtvflPVvactBrtOl; 

r  Initial  waypoint  la  0A0  BPY,  and  0  translation  1 

r  oitantailon  matrix  */ 

waypoints{0]jnatrtx{f)I0)m1; 

waypotntafOJjMtrixIiKojsO; 

waypotntsfOJjmatrtxPHO]^; 

waypotntatOpnatrtxtOIlJaO; 

waypointsfOlmatrtxll  I1)»1 ; 

waypolnta(0ljnatrtxt2I1J*0; 

waypointa{0]jnatrix(0H2)s0; 

waypolnis{0ljnatrtxIH2}s0-, 

aaipthiiWaa>l»HP)ri; 

r  bottom  raw  and  aoala  factor  •/ 

waypolnts{D)jnairix(3](0]s0; 

waypotnts{0]jnatrtx{3]t1H>; 
way|Mlnts|0)jnatrik(3!f’i--'h 
waypotntapqjiiaMxIsPfBl ; 

/*  poahton  augmentation  vector »/ 
vraypoints(0)jnalrix(0H3]a0; 
waypobitsfOpnatrtxIlK^O; 
waypointa(01jnatrix{2]i3)s0; 

/*  second  waypoint  la  soma  distance  straight  ahead,  to  cease  velocity  •/ 

I*  DupBoata  this  point  and  add  a  z-eomponent  ahead  •/ 
htOMst^H)  { 
torO-0;Md**)( 

waypolnts(1  pnairt*PKn*wsypoints(Oj.matrtxpHJj; 

} 

I 

waypolnta(l)jnatrtx{2p]z3*vinax*taoc;  r  this  distance  ahead  */ 

/*  computation  of  angles  tram  orientation  matrices  7 
tor(ka0;k«5;fc~)  { 

waypomts{kptdistawaypo4ms(kJjnatrixI0p]; 
waypolnts{ki.ydlstxwaypolnts(kl-matrix(l  ][3|; 
waypolnts{kpzdlstewaypolnts{k]jnatrtx{21[3]; 
waypolnts{k]pitchaas)n(-waypolnts{k]jnatrti({2H01}; 
waypobits[kl.yawaasln(waypoimsP(]jnatrix(2]Iiyooa(waypoints(k]4kltch)); 
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H  ((wypotntt(k)jn«trU11  I0yoot<waypo>nt^lp*>oh)H»  { 

H  ((mypoWa(kljmo0txt0t0yooo(woypoktta(klpftcii))»0)  { 

iaaypolnu(klJoi»fMb«<a«ln(awypolnnpi).  matrix}  HO  yooaQiwypotnta(fcl^t>cti))); 

I 

XM{ 

—ypolnmii|jBllmoin(-t)  latta<adn(t»typo<m«(kpnairtxftIOVPoaOaaypotn<a(l^pttch))); 

i 

» 

N  (Cw^olnt^tJW«gtj^IOyBO«(^ypo<nt<(fc]4>atJi))»0)  { 

OTypolnuWJo«»  l^(aaln(waypolnt»ftljn*trtxtl  I0yooa(waypotms(fclpttch))); 

I 

wypoln>»W«>^B<n(-1  )+M>*(a*in(waypo<nt*(k]Jiiairtxfl  JfOyoo«<wajrpo*»ta(k^pttcn))); 
) 
l 


r  printout  of  waypoint  matrtoas  and  orian  tattoo  angiaa  •/ 
toU halfclMS***)  { 
prtnttTn"); 

fcM(laO;U4;U*){ 

pmmnn“>; 

prtnMpur  ‘•jaaypoIntalH.naiirtyinBD; 

} 

prinMpUf  %M  %ar,««aypointa(k|4>Nctt,waypofcMs(k].yawliMypotnta(k]JW0); 

> 

oontagatonO: 


fdafina  pf  1.141582*536 
typadaf  doubt#  homog4[4][4]; 

homog4  *po«1  ,*poa2,*po«3,*poaa,*po»  lamp; 
douMa  <Mta6{B]T(MtacfS]; 

doubta  T1,hjaecji; 

doubta  alaMMIAWaft 

pool  *  (homog*  *)  mattoc(stz#o<(homog4)); 
poa2  a  (homoga  *)  malloc(*tzaof(homog4)); 
poal  a  (homog4  *)  mattoc(slzaof(t>oinog4)); 
poaa  a  (homog4  *)  malloo(aiz*ot(homog4)); 

tOC  (laO;U4;l*#) 

fer(M>^t4d*4) 

posa(1]Q]swaypoims{0ljnatrtx{I]0]; 
tor  (laO;M;i~) 

forQa0d«4d^) 

poa3{l]0>cwaypolnts{l]jnatrtxtlD]; 

for  (ZaO?«a?»«)  { 
poc_Mmpaposl; 
poalapoaa; 
poaaapoa.Mmp; 

po*_tampapoa2; 

po*2apo*3; 

pos3apo*_tamp; 


•/ 


tor  (la0^c4^4-») 
tor0a0j«4d*4) 

pos3fl]il}*w*ypotms{i  JjnatrfxPHn; 
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r  mwpuMInn  nf  — oaMwd  tra|rrtrrr  |rr“*r  * —  — IT-1"4*  *' 
r  inmates  *1  point  ol  jrajactory  to  fat  waypoint  *i 


•art; 


/*  Mi—  to  tadoa  minimum  acoalaraHnn 


tramaoriaa(0}.»msi(0)aaraypolntttO).xdlst; 
trajaeiariaa [Q]-ydtaitO)aamypolntsfl>l.ydist; 
trajaotoriat [QiJrtintoH— ypo*nt^°l-a*it; 


trajaomriaai0proapi)aswypointa{01.foil; 


*/ 


r  loop  ttwough  dwoa  Mm  wing  ttm  waypoints  to  eomptata  traioetory  */ 

daltsbjuteiaa wypimn[MMM*  wffolM)k1p<i«i 
daitafc.rdHhaaaypoint»[fci.yWal  waypo  InUQi-l  }.ydiat; 
da>Mb.xdl«t»— ypolnt^fc]  KM  —  ypnintaflsrtljdiat; 

Oaltato.pltcliaawypolniaM.piioh  waypoint  a{k-11.pitch; 
tetefciatewa ypnlnta|ifljoP  awyp  oin  w(fc-l  )jp<; 
daitatooiasrawaypoints(k).yaw-waypoints(k-1  J.ywn 
dWtac_xdl«t.waypoims(k«lt-idUl  woypointsftpBitat; 
daitac.ydl»Uwnypoint»(fc-»1}.ydtil  wsyp  oin  t^hlyWat; 
daitacJidlllwMaypolnti[h*l  }^disi-waypoints(k 
dahaopitcltawa ypointatiulEpitolt  waypolnt»(14-pt»ch; 
daliaBJOllaswypointt|fc+1  poll  waypointsMJot; 

Mtac.yaw*tt«ypoJnt*(k+l  Jyiwt  wwypuiiilBm.j)H, 
ajtfistswaypoints[k-1  ptdist; 

s-«ilsuwnypotnt«[h-H-rdlst; 
a-pltciwwaypoints(h-1 1-prtdi; 
ajoihnaaypoints(k--lpoa; 
a.yawswaypointa(k-l  J.yasr, 

diatancoaaqrt(pow(ciaiiacJKasC0«pow(dattBc.rdiai£)«poa<dattnfcgiist3)); 

tvmaxxtiatanoa/vmax;  r  flow  to  trawl  log  at  maximum  vaioctty  */ 

tl«g^tviiMX<2*taoe)  ?  rtaoe :  tvmax«2*taoc;  /*  lag  Is  aithar  twiea  minimum  accaiaratlon  Urn#  or  tmaxval  •/ 

hah*.1; 

Umaatlma  i  h*2*tau; 

tra|aolo<tos(l)jtdlst(2]s(1  /(2*pow(taUyQ))*(<iaKacjtfsf>tau/llag-daitab.xdte); 
tia>aotoriaa(I).ydlst{2WM2*pow(«»^)))*(<la*tac.ydi»r»auAlaB-dal«ab.y<Ust); 

tro)actortaa{llJdl*tpM1  P°**(tau^)))*(Oaltacjd>«tnau/tlag-doHato  Jdlsl); 

aaJaelo«tas(Ipriteh[2)a(142*Po"(inu>2)))‘(daitac4ftch*tau/llsg-daltab0Hch); 

tratactoriaa{I]j«tpM1/(2*Po*(tau12)))*(<la(tacjortau/tiag-4altab.ro<l); 

tra|aoto<tas{l}.yaw(2]s<1/(2‘pow(tau^)))*(daitac.yaw*tau/Uag-daitai>.yaw); 

ttaiactorlaappedlsqiHMaun^aBacjtdlsrtaumag  daliabJtdiatMoltabJrtistftwi; 

tra|acto<laa{f).ydl*t{1Mh/taur<<>aitac.ydJtt*tau/Usa-dattab.ydist)+dolta0.ydlst/tau; 

tr^aetoriao(llJdM(lHMmi)*(daliaa2«snauAiag-daiabjdlstHMtabadl*Vlau; 

tr^aoto<1«oII]^Heh(lMMaur(Mtt&pltBii‘tMiMag-daitab^iteti>*Ooitab^ileMau; 

traiaetoft«s[lpoU(1Mti/tau)*(dottaeJ«it*iau/tlaa-daKab./oa)*dsltabsolViau; 

traiactortaa{n.yaw(i  ]s(Mauntfattoc.y«rna4i/iiog4oKab.ya«r>+0aliab.yaw/tau; 

tra|actoriatp]J(dlst(0]apow(h£)*(dattocjufisf>tau/tiag-daitab.Miist)*<ialtabJ(dist*2*ti4ajcdist; 

tnjactorias{T].yd  lst(0)spow(l%2  )*(dattac.ydl«rtau  ftlag-rtai  tab  .ydlsty+dai  tab  .ydisrytt  *a.y dist; 

tra}actorias{TiidlstlO)*pow<h^)*(da»tacJdlst*tau/tlag-dnitab-xdlst>*da(tab.zdlar2*t»*aidlst; 

tr^aetofia^l]^Mci)(0)spew(hl2)*(daltae^tici),lauftiag-dattabg>tteh>*daltab^itoh*2*li«&piteh; 

tra|actortaa[lijoilt01*p^h^ndaltacjoBnau/Uag-daital»joll)*dalta0.ror2T»»njpil; 

Wiactorloa|||.yw»p|apow(li^(daliae.i)aonauWg  ■daitab.yaw)  i  d  altab.yaw*2*tv»a.yaw; 
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n  ■  ob)ooH  » ooli  100; 
dirina  1; 


r  road  am  pbcai  7 
fgwjigm  ■  «nmi  >  och. 
ipuijlwjf  ■  njun  « tow, 

■  t; 
t; 


r  eotuaui  0  Is  Ml  odgo  7 
r  y  coordinate  la 

rwUnlMf 

r  only  ONE  row  •/ 


rgnateal;  /* BUFFER  0  holds  1-byte 

rgnJbuf  a  (odor  *)  eoNoe  (rgnjogton.oMth'rgiLrogionJwtght, 

stzeoEchar)); 


otatuo  ■  toad  (M,  OET  XY_RQH,  Ergn);  /•  gM  die  rogion  7 

N  (OtMUO  k  E_NORMAL)  { 

ptlnM(*Erfor  roading  Ino  of  Mags.  Exiting  -Wn"); 

ed*1* 

) 


It 

7 


If  (polarity  s  1){ 

/•  oondnuo  roodlng  pinto  untU  Oral  agM  pixel  lo  found  1 
wMo  (00xfM«chor  >grU>uf)(OI}aOirMhoM)  EE  (ob)ecti-»oeWan))  { 
ob(oot1 1  ooli 1; 

rgnjogkMU  a  ob)aot1-*ool;  /*  column  0  lo  Ml  odgo  V 

ign  jogMuy  ■  aNocd-Mr,  /*  y-ooordinate  lo  tha  DOW  7 

rgnjagton-aidUt « 1;  r  entire  In*  */ 

rgnxeglon.haight  *  1 ;  /*  only  ONE  row  7 

rgtuateesl;  /•  BUFFER  0  holde  1-toyte 

rgnJxif  ■  (char  *)  oNloe  (rgnjagtorueidthTgiwregloiUteight, 

otzoo«(ehar)): 

OtMUO  ■  load  (M,  OET  XYJUN,  Efgn);  /*  gel  the  rogion  7 
If  (status  b  E_NORMaL)  { 

prtntf(~Error  roodlng  Hne  of  Imago.  Exiling 
oxH(l); 


) 


> 


} 


( 


wtiHo  (((OxffE((char  *>gnJ>ufX0D«lhresho4d)  EE  (otfoctl-aookan))  { 
otptHotolw; 

rgn  jogMu  a  objectioooi;  r  oolumn  0  lo  Ml  odgo  7 

rgnjoglon.y  a  objactl-xow;  /*  y-eoordinate  lo  Iho  ROW  7 

rgnjagloruoMdi  a  1;  /*  endro  Hno  7 

rgnxagkxiJwight  ■  1;  r  only  ONE  row  7 

rgruelzosl;  /•  BUFFER  0  hold*  1-byto 

rgnJbuf  a  (char  *)  oalloc  (rgnjaglon.wldth*rgnjoglonjwight, 

obooEetwr)); 

ototuo  a  ioctl  (M,  QET_XY_nQN,  Ergn);  r  got  tfw  rogion  7 
If  (OtMUO  la  E_NORUAL)  ( 

prlntfTEnor  roodlng  Hno  of  imago.  Exiting  —ViVil; 


) 


) 


If  (object! ->ool»n)  r  no  obieet  found  on  this  row  7 

prlntipnNo  object  found  on  thl*  row-*); 

also  { 

r  start  clwln  oodo  computation  7 

ivcaO;  r  numbor  of  atamams  In  diain  oodo  7 

rotartao bjacti ->row,  r  bolding  vartoblos  for  Man  of  chain  coda  7 

cstartaobjecti-»eol; 

prlntfpn  row  %4d,  ooi  %4dVstart,  cstart); 
portmMsraO; 
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«o{ 

/*  Qtab  pixel  */ 

HMUtou  ■  ab|ecfl » col;  /*  ooiumn  0 1»  Ml  edge  */ 

rgnjaglon.y  ■  objectl-arear;  r  y-eoordlnate  I*  Km  row  *r 

rg&iaglaiusldih  a  1;  r  entire  Ina  •/ 

ignjaglanJtelghi  a  1;  /*  only  ONE  roar  •/ 

rguteal;  r  SUFFER  0  hold*  i-(>yta  elements  •/ 

ignbuf  a  (altar  •)  oaUoe  (rgnjsgien.rt»i*fgiuagloiUialgM, 

ataao^etiar'); 

status  a  iecM  (M,  GET  XYJtQN,  Ergn);  r  gal  ths  region  V 

If  (statue  la  E_MOMML)  ( 

pflnUTErrer  reading  Ina  at  knags.  ExMng  -JnkO; 

) 


/*  attack  to  see  M  back  at  HUal  edge  pixel  •/ 

If  (wartaat|aan««aw  U  ostariannhtsrtl  ■  aaf  U  n»d| 
Eagel; 


dMn  a  (dhrkt«3)%a; 


ooltswipaob|sctl  jod; 
tor(M^U*){ 

i  (dMi){ 

ob|eetl-»ool**; 
break; 

uti|actt-»roa»  ; 
object! -»oot**; 


2: 

ob)aeti-»reat-; 


object!  .*co^-; 
braak; 
tea  4: 

object! -»<x>4-; 


ua  9: 

object! ->row**; 
objoctl<*oot-; 


iaa6: 

»b|aelUwaH; 


T: 

atjactl-noa^; 

Obtocll 


default: 
braak; 

> 

r  Grab  pixel  V 
rgnjaglonjt  a  object! -»ccl; 
rgnjeglon.y  a  object! -*row; 
rgnjeg  Ion. width  a  1; 
rgnjeg  Ion  .height  a  1; 
rgnjlnai; 

rgnJbuf  a  (char  *)  cal  toe  (rgnjeglon.wldth*rgnJsglo(U!eight, 

aizaof(char)); 

status  a  tool  (fd,  QET  XY  BON,  4rgn);  /•  get  the  region  */ 

If  (status  la  E.NORIML)  { 

prlntt(*Error  reading  Hna  of  tmaga.  Exiting  ~VtVt"); 

•xftO); 


r  edumn  0  Is  left  adga  V 
r  y-coordlnata  la  tha  ROW  •/ 
f  antira  Hna  V 
r  only  ONE  rear  */ 

f*  BUFFER  0  holds  1-byta  s laments  */ 
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> 

Wipatartty  —  iH 

If  (IOKff*((char  *)rgrLbuf)(0>threahoid)  ( 

tod; 

) 

dbhi  a  (dbtn*7)%b; 


> 


no**; 


> 

J 

If  ((MK((char  ')rgiibuf)(OJ)-cthrmhoU)  { 


etoa{ 

ditto  *  (dkrta«7)%2; 
ubj>ct1-»Tow»rowfmp; 
object!  »oohcohnni; 

> 


rgn2j»gionJt  *  rgmsgton  x;  r  ooluntn  0  to  toft  edge  •/ 

rgn2jegion.y  a  rgruegtany;  /*  y-ooordinets  to  the  ROW  'I 

rgnZregion. width  a  1;  /*  entire  Hne  */ 

rgn2jeglonJM4ght  a  1;  /•  only  ONE  row  •/ 

rgrOatesal;  f  BUFFER  0  holds  1-byta  itonwito  */ 

rgnZbuf  ■  (char  *)  caHoc  (rgnjegton.width*rgrLregionJiaight, 

st*eof(eher)); 


((char  *ygnZhufXO]  a  200; 

status  ■  lootl  (M,  PUT  XY  RON,  ftrgn2);  r  rastora  tha  region  */ 

If  (status  la  E.NORMAL)  { 

prtntfCError  ra storing  Hna  of  image.  Exiting  _wn“); 
aslid); 

> 

r  updata  Sptxals  in  chain  coda  */ 


If  (noc2){ 

dklnladiroutl; 

dlroutladirtn; 

dactatonadc(dlrlnl][diioiit1]; 
switch  (dlrtol)  { 


perimeter 


break; 


casa2: 

perintetoraparimeter*alpha; 


so  2: 

parimetarsperimeter*aqrt(1  *paw(alphaf2)); 


partmatsr**; 


casaS: 

parimaMrsparlmator*sqrt(1  *pow(a(pha^)); 
break; 

cased: 

partmatsrapartmatsr*alpha; 

break; 

par1matarapar1mator*aqrt(1  *pow(alphal2)); 
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) 

awMeh  (daeMen)  { 

mQ10)MD10}«pow((aipta*rowtwi)p)J); 
i«Di1  H»D11  |«paw<(alplw‘fmrtMnp)J)*ooHMBp; 
mtnpi^nQK2}+pow((«lph«*row»«np)J)*poi^oott»mp^0); 
M|H3)MnD131«paw((alptH‘rowiwnp)jrpow(ooUwnp4.0); 
> 


fltfQjflJrlJi  i)  { 

wBMP)MiiBPHpow((lpiwi^— iiwpMWpoWi^i  JP)h 

mQXIHnQXl  )-(po«i((alpha,ro«rtnwp)J)*eolteinp*(oolinnip-l  JBV2JB); 
mQB2)amOPHPow((alpha*rowtamp)J)*ooltainp*(eoMamp-l  .0)T2*oottamp-l  JiyfjO); 
■nQ13)MnD]t3KPo«K(alpha*rawiMnp)J)*paw((ooitMnp*(oe4twnp-lj0y2j0)^j0)); 

> 

brook; 
ifls  S* 

tor 

n(n(0)Krn[D!0]*pow<(alpha*rowtMnf))J}*oattwnp-, 

wlffll  >«m[|H1  )*pow((>lph«‘rowtwiH>)J?*oollnip*(oot»nip*1  J0)/2J>; 

■nO]I2)Bfn[]][2]*pow((alpha*rowtMnp)J)*oottMnp*(oolt*(it|>4l  .0)*(2X>*cottanip+1  jOy&O; 

m(]](3]sm(|13i'*paw((alplM*rowtMnp)J)*pow((oo<tMnp*(ooltMnp*1j0)/2J))lZ0); 

> 


) 

) 

•IM 

dtroutladlrin; 


}  white  (Iflag); 

If  (ne  <  2) 

prtnttpnNull  chain  coda*); 

ob}acn-»xb«xm(ipym[OnO]; 

objactl  -»  ytiar*m(0  Jf  1  J/m  [0J10 j; 

mu[0p)am[0p]; 

mu(ipM>; 

muIOIDaO; 

muPl[0)=m[2KO)-inIinorobJac«  -»xbar; 
mu(0U2)«m[0IPHn[0JIirot>fact1^yt)ar; 
fnudBD-nUHlHntiProbJactl-ybar; 

muppjrtn[3][0H3*obj*ct1-»*barmpIluiH2*m(1  pi*(paw(obfact1-»xbarl2))); 
mu(1JI2)rtn[ipH2*ob*actl->ybar*m[lHllHot'J*c*l->*1>ar*mI01t2IH2*tpow(obJacfi-»ybar^))*m(inOD; 

mup]|l  ]«n(2H1  H2*ofatact1-*xbor*mt1B1  IHobJoetl  -»yt>ar  mppD+{2*(po*v<oblecti  -»Kbar,2))*m[0](i  D; 

mu(0p)«n[0K3H3*obfact1  •»ybar*m[0](2]>»{2*pow(objact1  -»ybaf  ,2)*m[0U  1]); 

/*  invariant  moment  computation  for  latar  object  recognition  V 
tof(pe0;p«4;p+*){ 
ior(qaOai*a~)  ( 

gommoa(piq)B»1; 

nu[p][q]xmu[pUq]/pow(mu[01[0],gamma); 

I 

> 

phll1>mup«0)*nut0]I2]; 

phl(2}apow((not2J[0J-nu[0jra)^M*Pow(nu[11t1J^); 
pWPJspoiv((nut3P14*nul1][21WHK»a(<(3*Wi{2B1Hw[0Pft2); 
pN[4|gpow((nu(3B0]+nu[1H2])^)+pow((nu{2Hl]+nu(0P])£); 
r  phBSWnupil0>4*nuttpDnnul3H0hnulipir(po<»((n«PH0K»*ntil,iPWV3*Pow(«iu»PB1Hn»PPW)) 

♦<3*nu[2H1  Hui[0][3])*(nu[2JJl  l»nuI0p])*O*poa(((iuj[3][01«.nu[l  H2])4>-p°w<(nu(2I[l  ]*~i[OPIW)); 
phiJ6Xnuppj+nui0p])*(pow((nup]I0]*mi(ip])^)-pow((nu[2]I1!*nu[01I31)^)) 


H(nu(lIOHnu(l  I2]T(nu[2Ki  Hiu(0PD); 

^«PW»^Il>^I0innuPH0l^I1JPinpo^(nuPM0)^[iPD^)-3*po^(nupiIlHiut0pW» 
♦<Tnu{1I2Hiot5I0D*('wC2I1  l^ui(0B3D*<J*Pow((nu[3)J0}*f»u(l  I21)^)-pow((nu{2I1  }*no|0I3D^)); 

7 

r  computation  of  object  angle  */ 
f  m— rotor a2*mu(l  II  ]; 

danomtoatoramuPBPl-tauOTPl; 

H  (numerator aaO  U  denominator*:!)) 


Mil— .8*aton2(danomtoator,numarator); 

r  Mona Hr  of  arrays  Into  atntctura  format  7 
torfpalpaa***) 

obfeeti->phl[p]aphi(pl; 


) 


ara— mu(0I01; 


prtatfpnttSf  la  Mw  parlmator  of  tha  object  “.parknsWr); 
pflntfpnvBf  la  tha  oompactao—  of  Mm  object  “.objactl-ecompoctnosa ); 
prtatipnt%ef,%6f)  la  tha  (raw,oal)  of  tha  oontraid"lpb)ecti-»xbar,0hfacti-»ybar); 
» 


r - 7 

wold  get_poim(lnt  *eolnumJnt  *rowmjm>it  buff  Jot  Id) 

Dot  ootnum,  rownum,  buff.  Id;*/  /•  noun  oolumn  and 

{ 


•Mar  oont;  r  continue  Mag,  0  to  rapaot  */ 

lot  status,  I,  thrash;  rv 

r  mou—  routlns  wariaNas  7 

Mm  %,  y;  /*  old  moo—  poaitlon  */ 


lot 

tat 

tat 

tat 

tat 


tat 

tat 

tat 

XV_i#n_buf 


*1,  yl;  r  non  mou—  position  7 

pi;  /•  button  pnoa/raiaa—  Indicator  7 

p2,  p3;  r  button  pro—  location  7 

p4,  pS;  r  button  ralaaaa  location  •/ 

m status;  r  mousa  status  Indicator  7 

Mag;  /*  Mag  for  mou—  button  praas  7 

okflagaO;  /*  Mag  for  (sstoraOon  of  Imago  */ 

roar,  oodtl,  aobtl;  /*  roar,  loft  eoto—i,  right  oolumn  numbors  7 

rautup2,  rowdn2,  col;  r  top  raw,  bottom  raw,  oolumn  number  7 

rgn,  rgn2,  blank Jtaa,  blank _lina2; 


If  (buftaaO) 

dt67_lo_sal  («,  DT87  BUF0);  P  Prapora  to  talk  to  Buffer  0  */ 

at— 

«ttB7_lo_aal  (fd,  DTB7_BUF1 );  /*  Prapara  to  talk  to  Buffor  1 V 


r  Prapara  to  saw  and  blank  a  Una  of  Mw  Imago  7 
rgn  jag  Ion jc  a  0;  /*  column  0  is  left  edge  7 

rgnjaglon.arMth  a  768;  P  anti  ra  Una  7 

rgnjroglonJioight  a  1;  r  only  ONE  raw  7 

rgn^taoal;  r  BUFFER  0  holds  1-byto  otomonts  7 

rgnJbuf  a  (char  *)  oalloc  (rgnjaglon.srUtthTgruaglonJialght, 

blank Jtoajsgtoiui  a  0;  r  oolumn  0  la  lofl  adgo  7 

blank Jinsjeg(on.wt<tth  a  788;  r  ontira  Una  7 

blank JinajoglonJialght  a  1;  /*  only  ONE  raw  7 

blenkjmejtai  a  1;  /•  BUFFER  0  holds  1-byto 

blank JlnaJruf  a  (char  *)  oalloc  (btank_Unajaglon.iMtath*blank_iinajag>onJMight, 

iiziof(chtr)); 

rgnZragionjraO;  /•  column  0  is  laft  adgo  7 

rgniraglon.w»dth  a  1;  /*  antlra  Una  7 

rgnZraglonJMlght  a  812;  P  only  ONE  raw  7 

rgnLam  a  1;  r  BUFFER  0  holds  1-byto  oiomonts  7 

rgn2Jxjf  a  (char  *)  oalloc  (rgn2jeg  Ion. width  *rgn2j*gloruhelght, 

slzaof(char)); 

P  oolumn  0  la  loft  odgo  7 


blank Jlna2jag  Ion  jt  a  0; 
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btankJMLragionjridth  ■  1;  /•  antlra  Bna  •/ 

Mai*Jlna2j*gionJts4gM  ■  <12;  r  only  ONE  row  •/ 

MankjkM&ate  •  1;  r  BUFFER  o  hoMs  i-byta 

blank  Iina2jbuf  ■  (citar  *)  eaNoe  (Mai*  Mna2j«gton.wMtti‘U«*  lns2j«gioftJtaight, 

d***{dm))-. 


■touts jnMQ; 


*/ 


row  si; 

la-1;  /*  baposatta  vaiuao  */ 
ja-l;  I*  la  potato*  vaiuss  */ 

Mda  ouraorO; 

«o< 

<*{ 

Hal; 

yi«r. 

gst_iaouaa_iy(  Amstatua,  la,  By ); 
gst_button _pr«i«(0,  Amststus,  Apt,  Ap2,  Ap3); 
gat  button  raiaaaa(0,  Amststus,  Apl,  Ap4,  Ap5); 
if  (pi  *0){ 


«ae-o-J 

}  Wtti*  (laall  AA  ysayl  AA  Ipl); 

If  (okjlag— 1)  { 

status  a  iootj  (id,  PUT_XY_RQN,  Argn);  /*  rasters  Iris  ragkxi  7 

status  a  load  (M,  PUT.XY.RQN,  Argn2);  f  rastora  ths  rag  Ion  •/ 

if  (status  *  E.NORMAL)  ( 

printfCError  ra  storing  lina  of  Iraaga.  Exiting 
•HKIh 
» 

) 

ok_flaga1; 


If  (X>a10  ||  I«s502)  { 
eoHtlal-10; 
eoirt1ax*10;} 


If  (K10)  ( 
coittlax; 
eoirtlaialO;} 

aisa  ( 

oolltl  ax-10; 
eoirtlsx;} 


if  (y»10 1|  y«a7M)  ( 
rawup2ay-l0; 
rowdn2sy»10;} 
aisa  { 
if  (y<10)  { 

rowup2*y; 

rowtfn2sy«1Q;} 

aisa  { 

rowup2*y-10; 

rowdn2sy;} 


} 


rgnjsglon.y  ■  row;  /•  y-ooordlnata  Is  tha  ROW  •/ 

rgnjsglonji  a  eoiitl;  r  column  0  is  isfl  adga  V 

rgnjragton.wMttiaooim-oolirui;  /*  antirs  lina  7 

blank Jinajagtoax  a  eoktl;  /*  ooiumn  0  is  isft  adga  7 

bianfcj Inajag ton.wktth  ■  ooirti-coUti+1;  /*  antlra  Una  7 

status  a  loea  (M,  GET  XY  RON,  Argn); 

If  (status  taE  NORMAL)  ( 


r  gat  tria  ragion  7 
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> 

4W7  top _gain  (Id,  OTS7  OAW1);  r  eet  Input  tod  gUn  Id  1  •/ 

M7.*p  mask  (td,  OxFFh  /*  set  btt-wtse  display  auk  •/ 

dtoTjewsthni  (td); 

OMIiU; 

wMe  (oont  ■■  40)  { 

printrYttaMnartostton  ihraatxitd  (0-255): 
soanfT  »d*,Alhreah); 

M7_stop(fd); 

tor(M;  MH;M  P  Sat  1.UT  values  */ 
tffhdhreah) 

grean(l)nO; 

grean(l>£SS; 


status.  d«7  Inp  hlt(M,OTf7  WWTE, DTBT_Lirr. Drr57_LUT0, 0, 256, green); 
M  (status  fe  E_MOAUAL) 

p<tnd(~ Abnormal  LUT  opatHan 
dW7 _pasattmi  (Id); 

printfpnia  Ms  threshold  satisfactory?  (0  to  retry)  *); 


> 

d«7_stop  (fcO; 


voM  taka_Mo_pix  (M) 
hit  M; 

l 

IM  I; 

long  |; 


dt67_top_gain  (fd,  DTS7_OAM1);  r  sat  Input  a/d  gain  to  1  •/ 

dto7_diap_maak  (fd,  OxFF);  r  aat  bff-wtse  diaptay  maafc  */ 

d«7Jnp_chan  (td,  DTS7_CHAH0);  /*  uaa  oamara  0  */ 

dl67  inp  eat  (fd,  DT67  ACQO);  /*  pleturs  goes  to  buffer  0  */ 

dt57_dlsp_aal  (fd,  DT67_BUF0);  r  ealect  buffer  0  for  display  V 

printf  pnCAUERA  0  Uve  fenags.  Presa  any  key  to  freeze  _  "); 

dlS7_pesethru  (td); 

g||g||Qj 

dtS7_stop  (td);  r  alto  a  picture  */ 


tftS7Jnp_ohan  (td,  DT*7_CHAN1);  /*  uaa  camera  1  */ 

df67_lnp_sel  (Id,  DT57ACQ1 );  r  picture  gone  to  buffer  1  V 

dt£7_disp_s*i  (fd,  DTB7.BUF1);  /*  ealect  buffer  1  tor  display  V 

print!  pnCAUERA  1  Uve  Image.  Press  any  hay  to  fraszs  _  "); 
dtS7_pessthru  (td); 

dtff7_atop  (fd);  P  taka  a  picture  */ 

prtntfpntoToggllng  pictures.  Press  any  kay  to  quit  _  *); 
lor  (hd>;  tkbhHO;  i«H)  { 

HO-rO) 

dtS7_dlsp_sal  (fd,  DT57_BUFD);  P  select  buffer  0  tor  display  */ 


dtS7  dlsp  sal  (fd,  DT87JBUF1); 
fergaO;  {<200000;  {♦♦) 


P  satset  buffer  1  tor  display  V 


) 


> 

wtille  (kbhltO) 

geiefiQ; 


P  use  up  keys  In  buffer  */ 
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r — - - - 1 

nMMO 

i 


r  H  0_MMdWMMMta(_VIES1KOU>R)) 

poMfCan't  aitar  _waa16eotor  graphics  mod**);  */ 


_aaMo«*orl(OAmaxXjaaxY); 


yoftsstaOMacY/1.5; 

vaeaMamV/ZJOMGtfa; 

•atttaotolCjLIQHTBUNO;  /*  UOHTOREEN  V 

> 

r - - — - - . i  _ 

void  oomputa  vtaar  trana*orm<float  itwM,  float  gamma,  float  matrtxppD 

{ 

float  ttxtjaxQ; 


r  that^alaaafon.  gamma^rtmutfi  t 
at  a  akKthata); 
et  ■  ooa(ttiata); 
ag  a  fln(gaiima); 
og  m  ooatgafnma); 


r  iwmrfbnmtlon  from  i  to  2  V 
■Mtrtxtop]  a  -at; 

aiatrixflHO]  ■  -ct*og; 

matrispp]  a  -crag; 
maulx(OKli  a  ct; 
mauix(ill]  a  -at*eg; 
aatbPIlj  a  -at*sg; 
matrtx(0](2]  a  0; 
matita{iK2]  a  -ag; 
matrtxt2H2]  a  og; 

r  oomputa  vtawar'a  position  in  woiM  ooordinatas  */ 

vtaw_positton[0}zcg*ct; 

viawr_posltion[l)aog*at; 

vloar _poaitk>n[2>zsg; 

> 

/• - •/ 

void  oomputa  RPY  tnnsfotm((loat  yaw,  float  pitch,  float  rofl,  float  matrfi(3J[3D 

{ 

float  ap^p.ar.or.sy.ey; 

Int  I; 

float  *_ooord; 


ap  a  aln(piteh); 
cp  a  ooa(pitcii); 
ar  a  ain(roii); 
or  a  ooa(roa); 
ay  a  ain(yaw); 
oy  seoa<yaw); 


matilatop]  a  cy*cp; 
mautafipi  a  ay*cp; 
ma&txtzyo]  a  ap; 
mati1x(0Sli  ■  -ay*cr-cy*ap*ar; 
matriatlfl  j  a  cy*cr-ay*ap*ar; 
mattlx(2]Ii)  a  ep*ar; 
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—Miffqpi »  ■y‘M-ey*^*cf. 
—Hgg  m  •cy*f^y*«p‘or; 
■Mriippi  rnofTtr, 


) 

r  — 


•It  fast  natrtxI3I3D 


••■•InM; 

miomH; 

■•■•in(«Q; 

nmmi  •  •*•■; 

■MWxtlfO]  B  •«•*••; 
nMrtxpiO]  b  s* 

■wntxtOf i]  ■  m; 

MMta4HH>oc 

matrtx{0X2]  *  Ww; 
wHtipi  ■  m*m; 

MM4qp].M; 

> 

r - v 

void  transform _point(lnt  Ir.  float  pipi.  Int  pO{2D 
{ 


Int 

U* 

partpaettwa_aoala; 

/*  loop  oountaro  ’/ 

flora 

p_twnp(3lp2I3]; 
p_intarmadlato(2] ; 

for  (k*tr;fc>*0£-)  { 

r  parform  all  tranaframa  from  tr  to  0  •/ 

tor(h0^k>2^»H 
P_*M*pPHh 
for  0a0d<-2y««) 


p.MmptqBp.MmpplWUIkJjiMtrtetqQTpl  ffl: 

} 

1 

/*  (MKt  parfortn  parspoctiv*  wmfwnmian  Into  2-0  V 
parapactfta_acal— i/(p_tampci  )*<flst); 

pJntonnarflati(0)Bp_iamp(01*parapacttva_»gWi;  r  parapacthw  Maw  x-ooord  V 
pjmanpadMap|opLl— pWparapacfl—jraala;  f"  parspacttva  via**  y-coord  7 

/*  naw  aaala  2-0  data  to  M  atanport  •/ 
r  mate  aura  pO  Is  an  WTEOER  */ 
pO(0]Bp_lntanMtfaia(Orxacaia^mlfaat; 
pO(l  |»-p Jmaciwdla>a(1  l*yaoala  i  yoftaat; 


void  draw  all  DgurasO 

{ 

Int  l^z;  r  loop  ooumara  */ 

/*  _aaWna*tyta(OxfTFF);  •/ 

r  PM  of  shutda  trajectory  •/ 
for(iaO;k>26^«){ 

path[l)jnatilxtO]mtraiactoftaa(1).ydlatfO]; 
pathppnatri4l}*wojactof1aa{TUtfstf01; 
path[IpnattixI2)Btraiactortaa{1)j(dist{0]; 
tranaform_pofnt(l  lpM[QjnaMslpatliplo<(IJjnatrtac); 
prtntlOn%6fl%6ripattipiottIlJnatf1xt01^athpiottIpnatnz(1  Q; 


1. 1 


•MootoiteotorH  r  QUAY  */ 
oiovaio(pa»toio<(OpBairtxfOLpathpio<tO}.*Miri«(l  D; 


> 


tor  (WM-S^){ 

torO-Oioa^H 

ptoPJJBtortxflHfc 

) 

I 


ptttl^jMHrtKOtoSOO; 

l>tt(2lJMtota(lM00; 

puI>lJiMM4ZtoM0; 

Mto.tudlito'V'; 

Bk_napj.Y; 

uto  tMtptoY'; 


tranaJorte_poinX0j>ta(0)jnatrtxj>2ajnatrtx};  /*  origin  of  both  coordinate 


/•  world  coordinate  km  •/ 
ooior  a  12; 

aa«ootor(ooior);  r  UOHTCYAN  */ 
tor(te1;UaS;W*){ 

.■owiogtoniaitoWjto  urtHH; 
tran>tonn_polnt(0Ipts()pnatrtx4>2bjnatrtx); 
_tnoto(p2bJnatrtx{0]4>2b  jnati<x(1  D; 
_outgMd(axtojMaitIID; 

> 

) 

r - •/ 

void  display  traJactoryO 

{ 

M  I; 


MtQt 

vtow_thota_hom*a4&A; 

vtaw_gamma_lMmac30jO; 

vtew_gammasviaw_gam  majwmo; 

torOaO^m^a) 

anglaaflH); 

r  _daar»oraan(_OCLEAH8CHEEW);  •/ 

eompute_vtew_tran*fom>(vi«w_ttiata>  vtew jamma,  AAJOpnatrtx); 
oompote_RPY_trmn*»orm(ang  la*(2  J,angte*{01,ang  tes(1  ]>A(l  Jjnatrtx); 
oompute_Az£)_tranafonn(angiao{3]Tanglaa{4]rAA(2)jnalrb(); 

draw  oN  liguraaO; 

l 


v 

•I 

void  cteaaify  0 

( 

Ini  I; 

boat  n_out(*},  not; 
tor  (la 0;kS;laa) 
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printtPn**r4M4_bi{ID» 

n  otrt|0]  ■  (  .3223201—01)  *  dato_ln(0]  ♦  ( -4070904—00); 
n  MM(1)  ■  (  J71»*03o*01)  *  Mto_ln(1)  •  ( -.a06410W02); 
n  out{2]»(  .14291020*04)  •  Ma_ln(2]  «  (  QJOOOOOO— 00); 
n  oui(3]><  .17330040*04) * dato_ln(3] ♦  ( CLOOOOOOO— 00); 
n  oulf4)  ■  (  .2397001041)  *  doto_ln(4]  *  (  v2691105o*00); 


mh  *•  < •4KIM7MK)  •  n_eM{q: 
mi  «■  (  sHnimmo)  *  n_ou4ib 

Ml  *•  (  -.19747440*02)  *  n_out(2); 

Ml  mi  (  -J074733o*«2)  *  B_0U1(3]; 

Ml ««  ( -J174027— 02)  *  n.QMHl; 
pdnUTMl  >  W.M0: 

R_OtM(5]  ■  1  /{ 1«Mp(HMt)); 

mi  » ( sixm—ozu 

Ml  «a  (  *41177000*02)  *  n_OUt(01; 

Ml  «■(  *.1000000o*03)  •  n_om(11: 
mi  ~(  .34417220*02)  •  n_o«tpi; 

Ml  «a(  JIWlOlnM)  *  H_omPfc 
Mlw(  .1000000—01) » n  out(41; 

Ml  Mi  (  -.1*113530^)2)  *  ll.OUttSh 
prtnUTnM  «  VJSrjMfr 
njoutm  ■  1  /  (1«oxp(-noQ); 

Ml  ■  (  -49031190*02); 

Ml  *m  (  .1000000— 08)  *1  out(0]; 

Ml  «•(  .1000000—01) '  n_out(12; 

Ml«(  .1000000—03)  *  n_oul(2]; 

Mto(  .1000000-03) «i»  out(3]; 

Ml  «■  (  -^0379510—02)  •  n_out{41; 

Ml ««  (  0030704—02)  *  n_ou1(B]; 
printiCMt  m  %sr,Mt); 
iijmiCH  ■  1  /  (1*«xp(-not)); 

f*  IQ||§  OUlpUt  vtlUM  •/ 

dof.—mo) »(n_omtO)- (0.0000000— 00))/ (  J090099— 00); 
«Ma_out(1]  ■  (  n_out(7]  -  (  0.0000000—00) )  /  (  J099009— 00); 
pdimpnOutput  1  ■  %0f,  Output  2  m  %0 rvdoto  otrt(014al«_outf  1  D; 
prinMpnObjaet  It  Nunml  «0Tj(2j0MM_oul(0)^ola_aul(1D): 

) 

r - - —■ —  . . .  —  — 


r  'moumjxr  -  modulo  of  mouoo  rauHnoo  boood  an  aallo  to  "tot  SI*. 
W.  Clamant,  1/01.  */ 


1 

1 
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r* 


•_M*0 

-i 

»( 


prinUTMousa  drtvar  NOT  MCTALLED-MM"); 


NOi 


djtog  » i; 

•  ay  ( 10000, 10000  );  r 

•_ay  ( MM,  bMAXjt.  MU*_y ); 
i  ( tow am,  bnum .buttons *;  /•  mm  to 

■  owwO; 

NLk<OMK  0,770); 
aal_var1JUBits<  0,  *17  ); 


> 

f. 


^oursor(atotusAy) 


M  (Idrtvarjnatobsdjtog) 
raiwiKO); 

gat.mouao.ay  <  MM,  a.  y  ); 


> 


r  *toQ_fMM(r  I*  uaod  to  dtowito  wtwttMr  toa  mouaa  drtvar  Dm  baan 

Inaiaitod  and,  H  so,  lha  numbar  of  buttons  on  Mm  mouaa.  Also,  tha 
mouaa’a  position  to  ratal  to  Om  adddto  of  lha  seraan  aflar  Otto  cal. 
Upon  mum: 

H  mouaa  drtvar  NOT  InttoOad: 

ttotuaaO,  num.buttonaaO 
H  aiouta  drtvar  B  bistsllad: 

atatuf  1,  nuai  buttonsanumbar  of  mouaa  buttons 

V 

r  •  .I.  .  - —  v 

flag_rasat(  status,  num_buttona) 

Ini  ‘stolus.'num  buttons; 

( 

lagsjux  *  0;  r  Mouaa  Drtvar  systora  os*  0  */ 

lnt86(MOU8£_MT,  brags,  brag  ah 
‘status  ■  ragsjuv; 

*nuoi_buttons  a  ragsjubx; 

) 

r - — — — - ■—  ....  v 


/*  Show  or  Hida  tha  mouaa  cursor.  */ 

/*■■  —  —  —  —  — - ■■■-  ......  «/ 

show  ouraorO 

( 

if  (ldrtvar_lnatallad_ttog) 
ralum(0); 

ragsjuu  a  1;  r  Mouaa  Drtvar  aystam  cal  1  */ 

lnt06(MOU8E_MT,  brags,  brega); 

r - •/ 

Mda  ouraorO 

{ 
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tt  (idrtvar JnatoBod Jlag) 

"ton 1(0); 

m«JUBi»li  r  Houaa  Drtvar  systam  Ml  2  •/ 

tatMQtfOUNEJMT,  Ingt,  tugs); 

> 

r - v 


r  "gn_mouoo jcyO"  Mum  KM  horizontal  (a)  and  vortical  (y)  ptsol 

■unbar.  "aat_oMuoojryO"  npoatttons  too  noma  euraor. 

1 

r - 1 

gaLnomajqr  ( anna, «.  jr ) 
tot  "status,  %V. 

{ 

If  (Mrtvar JnaMad Jtog) 
mum(0); 

mgsjun  ■  S;  r  Houaa  Drtvsr  syaton  orf  J  */ 

lntM(llOU8EJHT.  data,  brags); 

•ataiui  ■  ragsjLtot; 

•x  ■  ragajLO; 

IfstaguA; 

> 

- — - . - - -  i 

aa«jnauaa_xy  ( x,  y ) 

M  a.* 

< 

N  (Idrtvar JnstaUad Jtog) 
ratum(0)7 

ragajun  a  4;  r  Houaa  Drtvar  ayatan  ea>  4  */ 

ragsjcxxax; 

rvgtxdx  i  y; 

InlMCMOUSE  MT,  brags,  traga); 

> 

r - v 


r  "gst_button_prossO"  and  *gat_button_raiaBaaO"  mum  tha  aana  vaiuaa 
tor  "status"  ragardtaaa  of  tha  valuo  aant  In  vartablo  "button": 
tlaiuaaO  -*  no  button  praaaad. 
natural  ■»  taft  button  praaaad. 
aiatuart  •*  right  button  praaaad. 
atotuaO  -a.  both  buttona  praaaad. 

Haaavat,  dta  V  and  V  poaWono  art  dad  to  tha  "button"  you  chooaa. 
buttonaO  -*■  toll  mouoo  button 
butionai  -a  right  mouoo  button 

FOr  "got_button_praaaO",  lha  vaiuaa  ratumad  to  *V  and  "y"  ara  too 


For  "got_button_ralaaooO".  tha  vaiuaa  mumod  In  "a"  and  “y*  ora  too 
horizon  tai  and  vartteal  pint  vaiuaa  of  too  toot  poahton  at  which  toa 
button  vraa  ralaaaad. 

"prass_count"  and  "ralaaaa_counr  ara,  raopactoraiy,  toa  numbar  of 
tonaa  tha  daaignatad  button  ana  praaaad  or  ralaaaad  alnoa  toa  laat  oaN 
to  this  function. 

*/ 

/• - */ 

gat_button_proaa  ( button,  status,  prass.oount,  x,  y  ) 
tot  button,  "status,  "proas  count,  *x,  *y; 

( 

If  (MrivarJnataHadJIag} 
ratum(0); 

rogsjuax  *  S;  /*  Houaa  Drtvar  ayatam  cal  S  */ 

rogajJbr  ■  button; 
lntM(MOUSEjNT,  brags,  brags); 
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‘XaragajLM; 
«y  a  wq»»<»; 


r - — - - — - - - 

9 a.  button  rataaaa  ( button,  sMus,  raf— _eount,  x.  y ) 

!«.“  button,*** ua.*Maaaa_oounf«.*y; 

( 

N  (Mhv.lnMM.Mg) 

ramn«(0>; 

rapULas  a  •;  r  Houaa  Ortaar  ayaMn  oaa  f  •/ 

ranaxta  a  button; 

MM(l«OUaC_MT.  tngi,  knffY, 

*mna  ragax.ax; 

tilir-r_r - a  mixla; 

*1  a  ngun; 

*y  aragajutx; 


r  ’‘mtJiorUJUnita’  aata  tha  Incfuafva  rang*  of  vatuaa  lha  curaor  can 
Mm  In  tha  horizontal  (x)  diraotian.  Thla  valua  apocMaa  tha 
looMon  of  *»  uppar  lafi  pixal  in  lha  Ifcrie  pixal  mouaa  ouraor 
(In  grapMca  moda). 

-aa»_vart_ Hmlta"  parionna  aiinUarty. 


*/ 


If  (Idrtvar JnaMMLItag) 


iatum(0); 

H«»« a  7;  /*  Houaa  Orfvar  ayaMn  oa>  7  */ 

lagsJua  a  minx; 
ragajLdx  a  man; 

MMtflOUKJNT.  Araga,  Araga); 


aat_van_amlta  ( winy,  waxy ) 
In.  winy,  waxy. 


{ 

M  (Idrlvnr Jnatallad Jlag) 
mum(0); 

ragaxax  a  •;  /*  Houaa  (Mvar  ayaMn  oaM  I  •/ 

raga-xxx  a  winy; 
lagaJLdx  *  maxy; 

IMM(HOUSE_MT.  Araga,  Arag a); 


aeraan.onO 

(  aafykfaomoda(  _VHESl6COCOR  ); 

> 

acraan  oftQ 

{  aatvtdaowocM  .DEFAULTHODE ); 

> 


